文档介绍：第一章卡尔曼滤波
卡尔曼滤波是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。简单来说，卡尔曼滤波器是一个'optimalrecursivedataproces角速度，9为加速度计经处理后得到的角度值,gyroacce
«为陀螺仪测量噪声，®为加速度传感器测量噪声，b为陀螺仪漂移误差，W和®相互gaga
独立，
此处假设二者为满足正态分布的白色噪声。
令Ts为系统采样周期,
得到离散系统的
0
0
0
0
状态方程和测量方程：
(1
X(k)二
10
X(k-1)+
co(k—1)+
gyro
o(k)T
gs
0
0
V(k)二[10]X(k)+o(k)
ia
0
0
同时，要估算k时刻的实际角度，就必须根据k-1时刻的角度值，再根据预测得到的k时
0
0
刻的角度值得到k时刻的高斯噪声的方差，在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值。在此，须知道系统过程噪声协方差阵Q以及测量误差的协方差矩阵R,对卡尔曼滤波器进行校正，Q与R矩阵的形式如下：
q
_acce
0
q_gyro丿
R=[r]
_acce
式中。q_acce和q_gyro分别是加速度传感器和陀螺仪测量的协方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，数值越小表明信任程度越高。在该系统中陀螺仪的值更为接近准确值，因此取q_gyro的值小于q_acce的值。当前状态：
r1
-T)
T
A二
<0
s
1J
，B=
s
0
1)
式中，
X(kIk-1)二AX(k-11k-1)+BU(k)
,X(kIk-1)是利用k预测的结果，X(k-IIk-1)是
k-1时刻的最优结果。
则有对应于X(kIk-1)的协方差为
P(kIk-1)二AP(k-IIk-1)AT+Q⑵
式中，P(k-11k-1)是X(k-1丨k-1)对应的协方差，AT表示A的转置矩阵，Q是系统
过程的协方差，式子(1)、(2)即对系统的状态更新。
则状态k的最优化估算值X(kIk):
X(kIk)二X(kIk-1)+K(k)(Z(k)-HX(kIk-1))(3)
其中H=[10],k为卡尔曼增益：
K(k)二P(kIk-1)Ht/(HP(kIk-1)Ht+R)(4)
此时，我们已经得到了k状态下最有的估算值X(kIk)，但是为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值，我们还要更新k状态下X(kIk)的协方差：
P(kIk)二(I-Kg(k)H)P(kIk-1)(5)
丁
其中I为单位阵，对于本系统则有I二1。当系统进入k+1状态时，P(kIk)就是式子(2)
的P(k-1Ik-1)。(3)、(4)、(5)式为卡尔曼滤波器状态更新方程。计算完时间更新
方程后，再重复上一次计算得到的后验估计，作为下一次的计算的先验估计，这样，周而复始、循环往复地运算下去直至找到最优的结果。
X=A*X+B*gyro
10
P=A*P*A+Q
10
Kg=P*H/H*P*H'*+R
11
X=X+Kg*(acc-H*X)
011
P=(I-Kg*H)*P01上述五个公式便是卡尔曼滤波的五条数学公式在本项目中的使用。但是，以上五个公式，仅仅只是矩阵形式。虽然可以在MATLAB里进行仿真，但是却不能使用单片机进行有效运算。
然后，将卡尔曼滤波矩阵形式转化为方程形式：
X=X+dt*gyro一dt*X
0102
X=X
02
P=P一dt*P一dt*(P一dt*P)+Q
**********
P=P一dt*P
120204
P=P一dt*P
0304
P=P+Q
042
Kg=P/(P+R)
11111
Kg=P/(P+R)
21311
P=P*(1-Kg)
01111
P=P*(1-Kg)
02121
P=P一P*Kg
0313112
P=P一P*Kg
0414122
X=X+(acc-X)*Kg
0111111
X=X+(acc-X)*Kg
0212112
这里，X01即为我们需要的角度卡尔曼滤波值。可以看到的是，卡尔曼滤波方程形式共有14个公式，同时很多参数的运算涉及浮点数，这对于单片机的高效运行时极为不利的，因此我们需要通过一些方法对卡尔曼滤波的公式进行化简，从而在不失精度的情况下，尽可能减小运算量。
需要注意的是，Q,R两个参数是关于传感器和系统的方差，他们随着系统的工作状况不同而会产生相应变化，对应到我们的系统，在车模运行状态