INS-GPS组合导航——经典卡尔曼滤波

最新推荐文章于 2024-08-02 18:20:17 发布

【杨（_> <_)】

最新推荐文章于 2024-08-02 18:20:17 发布

阅读量660

点赞数 24

分类专栏：信号处理代码实现文章标签：信号处理算法 matlab 学习

本文链接：https://blog.csdn.net/yjh_2019/article/details/140110249

版权

信号处理同时被 2 个专栏收录

42 篇文章 161 订阅

订阅专栏

代码实现

34 篇文章 27 订阅

订阅专栏

系列文章目录

《SAR笔记-卫星轨道建模》

《SAR笔记-卫星轨迹（三维建模）》

《常用坐标系》

《INS-GPS组合导航——卡尔曼滤波》

文章目录

前言

《INS-GPS组合导航——卡尔曼滤波》主要介绍了经典卡尔曼滤波以及扩展卡尔曼滤波算法流程。本文首先回顾一下经典卡尔曼滤波算法流程，然后针对几种常见的一维运动场景，编写对应的经典卡尔曼滤波算法。

一、经典卡尔曼滤波

经典卡尔曼滤波（Kalman Filter，KF）的局限性在于其只能拟合线性高斯系统。但其最大的优点在于计算量小，能够利用前一时刻的状态和当前时刻的测量值来得到当前时刻下的状态的最优估计。整个滤波算法主要涉及5个方程，分为两个阶段：预测阶段；校正阶段。

预测阶段涉及两个方程：

1、预测器方程（功能：由前一时刻的状态预测当前时刻的状态）

$\hat{x}_{n,n-1}=F\hat{x}_{n-1,n-1}+Gu_{n}$

2、预测器协方差方程（功能：得到当前状态估计的协方差）

$P_{n,n-1}=FP_{n-1,n-1}F^{T}+Q$

校正阶段涉及三个方程：

1、卡尔曼增益方程（功能：决定当前状态输出是更相信预测结果还是更相信测量结果）

$K_{n}=P_{n,n-1}H^{T}\left ( H P_{n,n-1}H^{T}+R_{n} \right )^{-1}$

2、状态更新方程（功能：卡尔曼滤波输出结果）

$\hat{x}_{n,n}=\hat{x}_{n,n-1}+K_{n}\left ( z_{n}-H \hat{x}_{n,n-1}\right )$

3、状态协方差更新（功能：更新状态的协方差更新）

$P_{n,n}=\left (I-K_{n} H\right )P_{n,n-1}\left (I-K_{n} H\right )^{T}+K_{n}R_{n}K_{n}^{T}$

除了上述主要方程，还涉及其他辅助方程：

1、测量方程（功能：描述状态量与测量量之间的关系）

$z_{n}=Hx_{n}$

考虑测量误差时，测量方程为

$z_{n}=Hx_{n}+v_{n}$

2、测量协方差（功能：描述测量量不确定性的，需要预先统计）

$R_{n}=E\left ( v_{n} v_{n}^{T}\right )$

3、过程噪声协方差（功能：描述系统建模误差，比如理论上匀速直线运动的模型，在实际处理中会因为各种干扰很难匀速直线，导致真实轨迹并非匀速直线运动，而预测器方程基于匀速直线模型建立，这样产生的误差即为过程噪声协方差 $w_{n}$ ，包括系统输入 $u_{n}$ 产生的影响，本文中 $w_{n}=Gu_{n}$ ，需要预先统计）

$Q_{n}=E\left ( w_{n} w_{n}^{T}\right )$

4、估计协方差（功能：卡尔曼滤波输出结果的协方差）

$P_{n,n}=E\left ( e_{n} e_{n}^{T}\right )=E\left ( \left ( x_{n} -\hat{x}_{n,n}\right ) \left ( x_{n} -\hat{x}_{n,n}\right ) ^{T}\right )$

其中更新后状态的协方差可以做如下化简

$P_{n,n}=\left (I-K_{n} H\right )P_{n,n-1}\left (I-K_{n} H\right )^{T}+K_{n}R_{n}K_{n}^{T}\\ =P_{n,n-1}-P_{n,n-1}H^{T}K_{n}^{T}-K_{n} HP_{n,n-1}+K_{n}\left (HP_{n,n-1}H^{T} +R_{n}\right )K_{n}^{T}\\ =P_{n,n-1}-P_{n,n-1}H^{T}K_{n}^{T}-K_{n} HP_{n,n-1}\\+P_{n,n-1}H^{T}\left ( H P_{n,n-1}H^{T}+R_{n} \right )^{-1}\left (HP_{n,n-1}H^{T} +R_{n}\right )K_{n}^{T}\\ =P_{n,n-1}-P_{n,n-1}H^{T}K_{n}^{T}-K_{n} HP_{n,n-1}+P_{n,n-1}H^{T}K_{n}^{T}\\ =P_{n,n-1}-K_{n} HP_{n,n-1}\\ =\left (I-K_{n} H \right )P_{n,n-1}\\$

这个方程看起来要精炼很多并且容易记忆，并且在许多情况下没什么问题。但是，在计算卡尔曼增益时的一个小误差（浮点截尾误差）可能给结果带来巨大的偏差。 $\left (I-K_{n} H \right )$ 的差可能因为浮点计算误差而使其结果不再是对称阵。这个方程在数值计算上并不稳定！

卡尔曼滤波算法流程如上图所示：一旦初始化完成，卡尔曼滤波首先预测下一时刻的系统状态，并且同时给出这个预测的不确定性。随后拿到测量结果，卡尔曼滤波会更新（或修正）这个预测值及其不确定性，并且同时再进行下一时刻的预测，以此类推。

二、场景用例

2.1、静止场景

场景描述：一维静止模型，目标距离1000m，状态转移矩阵 $F=1$ ，观测矩阵 $H=1$ ，持续时间10s，时间间隔0.005 s。

卡尔曼滤波输出距离输出误差

左上图对比显示了测量距离、预测距离、卡尔曼滤波输出距离以及真实距离情况，可以看出卡尔曼滤波输出距离具有更小的波动。右上图为输出距离误差。下图展示了距离估计的标准差，可以反映滤波输出距离的置信度。

距离估计标准差放大图

2.2、匀速运动场景

场景描述：一维匀速运动模型，起始距离1000 m，速度10 m/s，状态转移矩阵 $F=\begin{bmatrix} 1&dt \\ 0& 1 \end{bmatrix}$ ，观测矩阵 $H=\begin{bmatrix} 1 & 0\\ 0& 1 \end{bmatrix}$ ，持续时间10s，时间间隔0.005 s。

卡尔曼滤波输出距离输出误差

卡尔曼滤波输出速度输出误差

上面第一幅图对比显示了测量距离、预测距离、卡尔曼滤波输出距离以及真实距离情况，可以看出卡尔曼滤波输出距离具有更小的波动，上面第二幅图为输出距离误差。左下图展示了距离估计的标准差，可以反映滤波输出距离的置信度。上面第三幅图对比显示了测量速度、预测速度、卡尔曼滤波输出速度以及真实速度情况，可以看出卡尔曼滤波输出速度具有更小的波动，上面第四幅图为输出速度误差。右下图展示了速度估计的标准差，可以反映滤波输出速度的置信度。

距离估计标准差速度估计标准差

2.3、匀加速运动场景

场景描述：一维匀加速运动模型，起始距离1000 m，起始速度-50 m/s，加速度10 $m/s^{2}$ 状态转移矩阵 $F=\begin{bmatrix} 1&dt&0.5dt^{2} \\ 0& 1&dt\\0&0&1 \end{bmatrix}$ ，观测矩阵 $H=\begin{bmatrix} 1 & 0&0\\ 0& 1&0\\0&0&1 \end{bmatrix}$ ，持续时间10s，时间间隔0.005 s。

卡尔曼滤波输出距离输出误差

卡尔曼滤波输出距速度输出误差

卡尔曼滤波输出加速度输出误差

上面第一幅图对比显示了测量距离、预测距离、卡尔曼滤波输出距离以及真实距离情况，可以看出卡尔曼滤波输出距离具有更小的波动，上面第二幅图为输出距离误差。下面第一、二幅图展示了距离估计的标准差，可以反映滤波输出距离的置信度。上面第三幅图对比显示了测量速度、预测速度、卡尔曼滤波输出速度以及真实速度情况，可以看出卡尔曼滤波输出速度具有更小的波动，上面第四幅图为输出速度误差。下面第三、四幅图展示了速度估计的标准差，可以反映滤波输出速度的置信度。上面第五幅图对比显示了测量加速度、预测加速度、卡尔曼滤波输出加速度以及真实加速度情况，可以看出卡尔曼滤波输出加速度具有更小的波动，上面第六幅图为输出加速度误差。下面第五、六幅图展示了加速度估计的标准差，可以反映滤波输出加速度的置信度。

距离估计标准差放大图

速度估计标准差放大图

加速度估计标准差放大图

2.4、复合场景

场景描述：先静止10 s，起始距离1000 m,后匀加速运动10 s，加速度1 $m/s^{2}$ ，后匀速运动10s，然后匀减速运动10s，加速度-1 $m/s^{2}$ ，最后静止10s。

卡尔曼滤波输出距离输出误差

卡尔曼滤波输出速度输出误差

卡尔曼滤波输出加速度输出误差

上面第一幅图对比显示了测量距离、预测距离、卡尔曼滤波输出距离以及真实距离情况，可以看出卡尔曼滤波输出距离具有更小的波动，上面第二幅图为输出距离误差。下面第一、二幅图展示了距离估计的标准差，可以反映滤波输出距离的置信度。上面第三幅图对比显示了测量速度、预测速度、卡尔曼滤波输出速度以及真实速度情况，可以看出卡尔曼滤波输出速度具有更小的波动，上面第四幅图为输出速度误差。下面第三、四幅图展示了速度估计的标准差，可以反映滤波输出速度的置信度。上面第五幅图对比显示了测量加速度、预测加速度、卡尔曼滤波输出加速度以及真实加速度情况，可以看出卡尔曼滤波输出加速度具有更小的波动，并且在加速度跳变处，卡尔曼滤波输出也能跟踪上，上面第六幅图为输出加速度误差。下面第五、六幅图展示了加速度估计的标准差，可以反映滤波输出加速度的置信度。