TN07 - 卡尔曼滤波在姿态解算和位置估计中的应用

TN07 – 卡尔曼滤波在姿态解算和位置估计中的应用

1. 卡尔曼滤波

参考：ttp://zh.wikipedia.org/wiki/卡尔曼滤波

对象模型

xkAkxk1Bkukwk （1）

ykCkxkvk （2）

yk为观测量，wk和vk分别为过程噪声和输出噪声，其中xk为状态量，假定其为均值为0，

协方差矩阵为Qk和Rk的正态分布噪声。

wkN0,Qk

vkN0,Rk

滤波算法

预测

ˆkAkxˆk1Bkukx (状态预估)（3）

TPkAkPk1AkQk (状态协方差预估)（4）

误差

ˆkekykCkx （测量误差）（5）

TSkCkPkCkRk （测量误差协方差）（6）

T1LkPkCkSk （最优卡尔曼增益）（7）

更新

ˆkxˆkLkekx （状态更新）（8）

PkILkCkPk （状态协方差更新）（9）

其中协方差准确的反映了估计的协方差

ˆkPkcovxkx

Skcovek

2. 扩展卡尔曼滤波

扩展卡尔曼滤波应用卡尔曼滤波的算法于非线性对象，对象模型为

xkfxk1,uk,wk （10）

ykhxk,vk （11）

其中变量的意义与卡尔曼滤波对象模型相同。在卡尔曼滤波算法中，分别用非线性函数估计对象的状态和观测量，用非线性函数的Joccob矩阵进行状态和协方差的更新运算，如下

预测

xˆkfxˆk1,uk,0 PATkkPk1AkQk 其中

Afkxxˆk1,uk,0误差

ekykhxˆk,0 SCTkkPkCkRk LPCT1kkkSk 其中

(状态预估)（12）

(状态协方差预估)（13）

(状态转换矩阵)（14）

（测量误差）（15）

（16）

（17）

（测量误差协方差）（最优卡尔曼增益）

Ckhˆk,0xx

(输出矩阵)（18）

更新

ˆkxˆkLkekx （状态更新）（19）

PkILkCkPk （状态协方差更新）（20）

3. 姿态解算

姿态更新模型

1qMq2

（21）

其中

0xMyzx0yz0zyzyxx0

（22）

qq0,q1,q2,q3T，为姿态的四元素表示。

实际应用中，角速度为可测量，另外加速度和指南针为可观测量。建立离散形式为

qkAkqk1wk （23）

akTqkag （24）

mkTqmg （25）

其中

1111dtdtdtxyz222111dt1zdtydtx22Ak211dt1dt1xdtyz222111zdtydtxdt1222

（26）

如果采用精细微分法，则有

qkqk1e1Mdt21122133qk1IMdtMdtMdt2848

（27）

ag和

mg为地面坐标系下的加速度和磁场矢量，Tq为地面坐标系到机体坐标系的转换

矩阵

222q0q12q2q3Tq2(q1q2q0q3)2(q1q3q0q2)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2q22q32q02q12q22q12q0

2q1T2q2q12q32q22q12q02q32q02q1

2q2T2q1q22q02q12q22q32q02q32q2

2q3T2q0q32q12q02q32q22q12q22q3

求取输出Jaccob矩阵

akakakakak,,,qq0q1q2q3TTTTag,ag,ag,agq1q2q3q0（29）

mkTTTTmg,mg,mg,mgqq0q1q2q3（30）

4. 位置估计

对象模型

xk1dtv01kak00dt22xk1wdtvk1k1ak1 （31）

xkvykvkkak

（32）

其中wk和vk分别为过程噪声和观测噪声