EKF: 计算雅克比矩阵:; 计算状态变量的一部预测及协方差矩阵:; 计算卡尔曼增益: 计算观测值: 状态和协方差矩阵更新:; UKF: 构造sigma点集与权值,对于n维状态向量,一共有2n+1个sigma点 ; |
计算sigma数据点集的一步预测值: 计算状态变量的一步预测及协方差矩阵:; 计算新sigma点集:其中表示矩阵的第i列 利用新sigma点集代入观测方程: 计算观测估计值的均值: 计算自相关矩阵: 计算互相关矩阵: 计算卡尔曼增益: 状态和协方差矩阵更新:; CKF: 构造容积点集,对于n维状态向量,一共有m=2n个容积点: 其中通过对协方差矩阵进行cholesky分解获取,如式:
传播容积点: 计算状态一步预测值: 计算状态预测方差矩阵: 计算方差矩阵平方根: 构造新容积点集: 传播新容积点: 计算观测估计值: 计算自相关矩阵: 计算互相关矩阵: 计算卡尔曼增益: 状态和协方差矩阵更新:; |