TN07 – 卡尔曼滤波在姿态解算和位置估计中的应用
1. 卡尔曼滤波
参考：ttp://zh.wikipedia.org/wiki/卡尔曼滤波 对象模型
xk?Akxk?1?Bkuk?wkyk?Ckxk?vk（1） （2）
其中xk为状态量，yk为观测量，wk和vk分别为过程噪声和输出噪声，假定其为均值为0， 协方差矩阵为Qk和Rk的正态分布噪声。 滤波算法 预测 误差 更新
?k?x?k?Lkek x?kek?yk?Ckx?k?Akx?k?1?BkukxTPk?AkPk?1Ak?Qk
wkvkN?0,Qk? N?0,Rk?
(状态预估)（3） (状态协方差预估)（4）
（测量误差）（5）
（测量误差协方差）（6） （最优卡尔曼增益）（7）
TSk?CkPkCk?Rk
T?1Lk?PkCkSk
（状态更新）（8） （状态协方差更新）（9）
Pk??I?LkCk?Pk
其中协方差准确的反映了估计的协方差
?k? Pk?cov?xk?xSk?cov?ek?
2. 扩展卡尔曼滤波
扩展卡尔曼滤波应用卡尔曼滤波的算法于非线性对象，对象模型为
xk?f?xk?1,uk,wk?yk?h?xk,vk?（10） （11）
其中变量的意义与卡尔曼滤波对象模型相同。在卡尔曼滤波算法中，分别用非线性函数估计
对象的状态和观测量，用非线性函数的Joccob矩阵进行状态和协方差的更新运算，如下
预测 其中 误差 其中 更新
?k?x?k?Lkek x
?k?f?x?k?1,uk,0?xTPk?AkPk?1Ak?Qk(状态预估)（12） (状态协方差预估)（13）
Ak??f?k?1,uk,0??x?x
(状态转换矩阵)（14）
?k,0?ek?yk?h?xTSk?CkPkCk?Rk
（测量误差）（15）
（测量误差协方差）（16） （最优卡尔曼增益）（17）
T?1Lk?PkCkSk
Ck??h?k,0??x?x(输出矩阵)（18）
（状态更新）（19） （状态协方差更新）（20）
Pk??I?LkCk?Pk
3. 姿态解算
姿态更新模型 其中
?0???xM??????y?????zT1q??M?q2（21）
?x0
?y ??z0?z??y?z??y????x??0??（22）
?x
实际应用中，角速度?为可测量，另外加速度和指南针为可观测量。建立离散形式为 其中
??1??1?dt?2xAk???1?dt?2y?1??zdt?21??xdt211??zdt21?ydt2qk?Akqk?1?wkq??q0,q1,q2,q3?，为姿态的四元素表示。
（23） （24） （25）
ak?T?qk?agmk?T?q?mg
1??ydt2 1?zdt211??xdt21???zdt?2?1??ydt??2?1?xdt??2?1??
（26）
如果采用精细微分法，则有
qk?qk?1e1?M?dt2
1122133??qk?1?I?M?dt?M?dt?M?dt?2848????（27）
ag和mg为地面坐标系下的加速度和磁场矢量，T?q?为地面坐标系到机体坐标系的转换矩阵
?q?q?q?q?T?q???2(q1q2?q0q3)?2(q1q3?q0q2)?20212223
2(q1q2?q0q3)222q0?q12?q2?q32(q2q3?q0q1)
2(q1q3?q0q2)??2(q2q3?q0q1)?222?q0?q12?q2?q3?（28）
计算偏导
?2q0?T???2q3?q0???2q2?2q1?T??2q2?q1???2q32q32q0?2q12q2?2q1?2q0?2q2?2q1?? 2q0??2q3?2q0?? ?2q1??
?T??2q22q1?2q0??q???2q12q22q?32??2q02q3q? ?22???T??2q32q02q1?
?q???2q0?2q32q?23??? ?2q12q22q3??求取输出Jaccob矩阵
?ak
?q????ak,?ak,?ak,?ak???q?q?0?q12?q3?????Ta?T?T?T???qg,ag,ag,ag?0?q1?q2?q3? ?mk?q????Tm?T?T?T?g,?qmg,mg,m??qg? 01?q2?q3?
4. 位置估计
对象模型
?xk???1dtdt22??xk
?v?k????01dt??1?????v?k?1??wk?ak????001????ak?1???
?xk?y?k??vk???vk
?ak??其中wk和vk分别为过程噪声和观测噪声
29）30）31）32）（
（
（
（
相关推荐：
loading