非线性滤波及航天应用(一)——Kalman滤波器
介绍基础的 Kalman 滤波器以及在 Matlab 中的实现。
基本原理
基本思想
Kalman 型滤波(卡尔曼滤波)是一种基于系统状态空间模型和统计估计理论的递推滤波方法,主要用于从含有噪声的观测数据中估计动态系统的真实状态。
其基本思想是:利用系统的状态转移模型,根据上一时刻的状态估计预测当前时刻的状态;再利用当前时刻的实际观测值,对预测结果进行修正,从而获得更加准确的状态估计。
Kalman 滤波的核心是将两种信息进行融合:
- 模型预测信息:根据系统动力学模型和上一时刻的状态估计,预测当前状态。
- 测量观测信息:根据传感器获得的实际观测数据,修正预测状态。
通过综合考虑预测误差和测量误差的统计特性,Kalman 滤波能够在噪声环境下实现对系统状态的最优估计。
状态空间数学模型
式中:
系统过程噪声和量测噪声的统计特性为:
其中:
:过程噪声协方差矩阵。 :测量噪声协方差矩阵。 :Kronecker 符号。
通常还假设过程噪声与测量噪声相互独立。
状态方程中过程噪声经过了矩阵
而不是直接使用
算法步骤
确定初始条件
Kalman 滤波需要给定初始状态估计和初始估计误差协方差:
其中:
:初始状态估计。 :初始状态估计误差协方差。
如果初始状态已知得比较准确,则可以设置较小的初始协方差;如果初始状态不确定性较大,则应设置较大的协方差。
Kalman 滤波迭代过程
整个算法分为两个阶段:时间更新(预测)和测量更新(校正)。
- 时间更新(预测阶段)
利用上一时刻的状态估计,预测当前时刻的状态。
(1)状态预测
该式表示:根据上一时刻的状态估计以及当前控制输入,预测当前时刻的状态。
(2)估计误差协方差预测
该式用于计算当前预测状态的不确定性。
2. 测量更新(校正阶段)
获得当前时刻的实际观测值
(1)计算观测残差
观测残差表示实际测量值与理论预测测量值之间的差异。
(2)计算残差协方差矩阵
(3)计算 Kalman 增益
Kalman 增益决定了观测残差对状态估计的修正程度。
(4)更新状态估计
(5)更新估计误差协方差
至此,第
实际编程时,也可以使用 Joseph 形式更新协方差,以改善浮点运算中的数值稳定性。
编程实现
编写 Kalman 滤波函数
假设系统状态维数为
为了兼容时变系统,下面使用三维数组存储各时刻的系统矩阵。
1 | function [x_filt, P_filt, K_hist] = KalmanFilter( ... |
这里有几个需要注意的地方:
x_filt(:,1)表示初始状态估计,x_filt(:,k+1)表示第 次观测更新后的状态估计。P_filt(:,:,k)表示对应时刻的估计误差协方差。K_hist(:,:,k)保存每次迭代的 Kalman 增益。- 代码采用 Joseph 形式更新协方差矩阵,避免直接使用简化公式可能产生的数值对称性问题。
构造一个具体数值算例
为了验证上述算法,考虑一个二维运动系统:
其中,
假设采样时间为
假设没有控制输入:
过程噪声作用矩阵取:
假设只能测量位置:
噪声协方差矩阵取:
初始条件设置为:
假设获得以下五组位置测量数据:
可以编写以下 MATLAB 测试程序:
1 | clc; |
运行后,x_filt 的第一行表示位置估计,第二行表示速度估计,将原始测量数据与滤波后的估计结果绘制在同一张图中。
即使传感器只能测量位置,Kalman 滤波也能够利用系统的运动模型,对无法直接测量的速度进行估计。