23阶扩展卡尔曼求姿态、位置、速度
23阶卡尔曼总结。一、首先给出卡尔曼的五个公式:卡尔曼算法的本质为:根据上一刻的最优值估计此刻的预测值,实际测量此刻的测量值。将预测值和测量值加权和即此刻的最优值。首先离散状态空间表达式为:根据上一刻估计此刻的预测值:P为估计误差协方差矩阵,协方差矩阵为X各个元素之间的协方差值组成的矩阵。求卡尔曼增益,即加权系数。Kg= P(k|k-1)HT/(HP(k|k-1)HT+R)R为W1的协方差矩阵,Q为W2的协方差矩阵。求此刻的最优值:R是表征测量误差的,R越大说明测量越
原创
2021-04-23 10:41:18 ·
754 阅读 ·
0 评论