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。

然后进行