KalmanFilter在GPS定位中的应用

Kalmanfilter是一个离散线性差分系统。系统状态前后存在一定的关系，除了状态转移（多个系统变量对下一个状态的影响），还有过程噪声和测量噪声。Kalmanfilter采用递归收敛的方式，能预测下一个系统状态或使输出的结果更可靠稳定。

Gyro陀螺仪是传感器数据的来源之一，由于它的精度与温度相关，所以应用Kalmanfilter对其进行预测和校正。这里其输出主要是校正的数据。

Gyro的状态方程为

gbias = a0 + a1 * T + a2 * T^2 + a3 * T^3

若T1=T, T2=T^2， T3=T^3.

----------以下是看别人写的代码的理解，有点蒙掉的感觉，因为没找到第一个差分量的含义，所以以下有关第一个变量的东西可能有误

gbias是Gyro的偏移量数据，则kalmanfilter的状态转移矩阵的大小为5*5，系统状态矩阵（向量）为5*1。测量矩阵为1*1

则

A = 

-1 1 T1 T2 T3

 0  1  0   0   0

 0   0  1   0   0

 0   0  0   1   0

 0   0  0   0   1

x  = (0, a0, a1, a2, a3 )T

H = （1, 1, T1, T2, T3）

拿到温度T， 及Gyro直接测量到的bias值。 

时间更新，估计：

首先由温度T，计算出mbias，然后设置状态转移矩阵A。

然后进行测量更新，校正：

测量更新measurementUpdate，计算直接测量bias值与计算值mbias引起的误差协方差矩阵mMatMainCovariance和系统矩阵mMatCorrection的更新。

由新的a0, a1, a2, a3再次计算bias，得到校正的bias值。

此应用中，其中一个先验估计值bias由T计算出，使用旧的系统变量ai,然后由测量值bias，计算出最优的系统状态变量ai,并再次计算出最优的估计值。

x(0|0) = (0, A0, 0, 0, 0),      A0 = bias

P(0!0) = identity（0.1, 0.1, 1e-3, 1e-6, 1e-9）

predict:

x(1|0) = A*x(0|0)

P(1|0) = AP(0|0)At + pnoise

correct:

Kg = P(1|0)/(P(1|0) + noise)

x(1|1) = x(1|0) + correctMatrix ， correctMatrix = kg*error， error = measurement - x(1|0)[0]

P(1|1) = P(1|0) - Kg*H*P(1|0)

-----------------------------------------------------------

看的太累了，附上两张GPS位置和gyro bias效应的kalmanfilter的示意图

系统变量

x(t) = (x, v, a)

状态转移矩阵

A = 

1, t, t^2/2

0, 1, t

0, 0, 0

测量输入变量可以有

z = (x, v)

B, 为0，H,单位矩阵，R,Q高斯随机噪声。

估计和校正x，v。

设置x(0|0),P(0|0),就可以开始Kalmanfilter的过程了。

这个陀螺仪的模型不太熟悉，以后再分析。