改进联合滤波在GPS/DR组合定位中的应用

　　DR局部滤波器的观测值取误差补偿后的DR估计的航向角θ和位移s，分别为

　　式中εθ和εs分别为DR航向角和位移的观测噪声，均值为零，方差分别为 。得到一阶线性化离散方程为

　　式中

　　Wi（k）为观测噪声矩阵，H（k）为h（k-1）对Xi（k-1）的一阶微分

　　DR观测值受传感器误差累积影响，因此当GPS信号良好时，用GPS观测值来校正DR观测值。取λθ（k）和λs（k）分别为GPS的航向和里程观测值与DR对应观测值的误差，为抑制GPS的噪声，对其进行限幅处理，分别得到λ′θ（k）和λ′s（k），DR观测值的自适应误差反馈校正过程如下。