GPS定位中的卡尔曼滤波算法

卡尔曼滤波：
卡尔曼滤波由预测和校正两部分组成。
预测部份又称时间更新过程，是在上一个历元(k-1)状态估计值的基础上，利用系统的状态方程来预测当前历元(k)的状态值。
校正部分又叫测量更新过程，它是利用实际测量值来校正经上一步得到的状态先验估计值。
卡尔曼滤波递推算法如下：详细介绍可见：卡尔曼滤波公式及各参数意义

卡尔曼滤波定位算法
状态方程：对于行人、汽车、船舰等，GPS接收机运行情况可用八个状态向量来表示即三个位置分量（x,y,z)，三个速度分量(Vx,Vy,Vz)和两个接收机时钟变量（钟差和频漂）。
常系数状态转移矩阵A和协方差矩阵Q可由已知参数求得。
测量方程：
1、由于GPS卫星的空间位置和时钟钟差状态向量通过GPS伪距得以体现，故卫星伪距测量方程式可作为卡尔曼滤波测量方程。
2、GPS系统状态还包含卫星速度、接收机速度。故以接收机速度和时钟钟漂为函数的卫星多普勒频移测量公式也可作为测量方程组的一部分。
两部分经线性化之后可组成卡尔曼滤波测量方程。
GPS测量值误差方差
从理论上讲，一个测量值的误差方差可以认为卫星星历误差、电离层对流层误差、卫星及接收机钟差、多路径误差、天线相位中心偏差及噪声等各部分误差方差之和。因此，可以根