卡尔曼滤波理解及代码分析

鉴于网上的代码以及分析的各种错误，所以写一个正确的详细的分析。

过程方程以及量测方程

X(K)=AX(K-1)+BU(K-1)+W(K-1)

Z(K)=HX(K)+V(K)

说明，下面带T的表示转置。

下面的程序主要针对MPU6050的姿态角的滤波。

首先建立的是过程方程，这里的状态变量是angle以及Q_bias,角度以及陀螺仪的漂移。

那么已经建立了这里的预测方程，没有加上噪声。

上面的代码就对应着预测方程。对应着卡尔曼滤波的五个公式的第一条：X(k|k-1)=AX(k-1|k-1)+BU(k)

这里再分析第二条公式，P(k|k-1)=A P(k-1| k-1) AT+Q。可以在之前看出，A=[1,-dt;0,1]。而Q的定义如下：

因为角度噪声和陀螺仪的角速度的漂移噪声相互独立，所以为一个对角矩阵。然后，Q_angle,Q_gyro再程序开头已经给出。所以设P=[a,b;c,d]

的出一个更新的式子， [acbd]=[10−dt1][acbd][1−dt01]+[Qangle00Qgyro] $$/begin{bmatrix}a & b // c & d/end{bmatrix}=/begin{bmatrix}1 & -dt // 0 & 1/end{bmatrix}/begin{bmatrix}a & b // c & d/end{bmatrix}/begin{bmatrix}1 & 0 // -dt & 1/end{bmatrix}+/begin{bmatrix}Qangle & 0 // 0 & Qgyro/end{bmatrix}$$ 最后的到的更新的方法 [acbd]=[a−c∗dt−b∗dt+d∗dt∗dtc−d∗dtb−d∗dtd]+[Qangle00Qgyro] $$/begin{bmatrix}a & b // c & d/end{bmatrix}=/begin{bmatrix}a-c*dt-b*dt+d*dt*dt & b-d*dt // c-d*dt & d/end{bmatrix}+/begin{bmatrix}Qangle & 0 // 0 & Qgyro/end{bmatrix}$$ 所以看代码，可以看出写的极为的不合理，但是都是这样写的，先看一看。

对照上面的公式，还是可以看出来，PP就是[a,b;c,d]的，但是注意，Pdot只是矩阵运算的中间值，但是不知为什么要叫成Pdot，误人子弟。而且最大的错误在于这样写，Q乘以了一个dt，但是最后并不会怎么影响，因为Q也是初始给的一个值而已，但是这样写还是有问题的，还是按照推导来写比较好。

再是第三个公式来计算卡尔曼增益，Kg(k)= P(k|k-1) HT / (H P(k|k-1) HT + R)，所以这里要做的就是再建立一个量测方程，这里测量的值是加速度计算出来的角度值，所以 Accelangle=[10][angleQbias]+R $$Accelangle=/begin{bmatrix}1&0/end{bmatrix}/begin{bmatrix}angle//Qbias /end{bmatrix}+R$$ 所以H=[1 0],卡尔曼增益就是一个二维向量[k0,k1]T。

直接带入计算第三个公式。

基本还算比较清楚，但是命名的话真的不忍心吐槽。

再看第四个公式，X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1)).

第五个公式对PP进行更新，P(k|k)=（ I-Kg(k) H） P(k|k-1)；

已经不忍心吐槽他的命名了。到这里基本分析完毕，至于卡尔曼滤波的证明推导，可以参考其他，这里只是分析代码，但是网上基本都是这样写的，有分析的，但是要么就是直接甩代码，要么就是分析的很多错误，太乱，决定写一个完整的正确的分析mpu6050卡尔曼滤波的程序。