介绍基础的 Kalman 滤波器以及在 Matlab 中的实现。

基本原理

基本思想

Kalman 型滤波(卡尔曼滤波)是一种基于系统状态空间模型和统计估计理论的递推滤波方法,主要用于从含有噪声的观测数据中估计动态系统的真实状态。

其基本思想是:利用系统的状态转移模型,根据上一时刻的状态估计预测当前时刻的状态;再利用当前时刻的实际观测值,对预测结果进行修正,从而获得更加准确的状态估计。

Kalman 滤波的核心是将两种信息进行融合:

  1. 模型预测信息:根据系统动力学模型和上一时刻的状态估计,预测当前状态。
  2. 测量观测信息:根据传感器获得的实际观测数据,修正预测状态。
    通过综合考虑预测误差和测量误差的统计特性,Kalman 滤波能够在噪声环境下实现对系统状态的最优估计。

状态空间数学模型

式中:
时刻系统状态向量
状态转移矩阵
控制输入矩阵
控制输入向量
实际观测向量
观测矩阵
系统噪声矩阵
系统噪声
观测噪声

系统过程噪声和量测噪声的统计特性为:

其中:

  • :过程噪声协方差矩阵。
  • :测量噪声协方差矩阵。
  • :Kronecker 符号。
    通常还假设过程噪声与测量噪声相互独立。

状态方程中过程噪声经过了矩阵 作用,因此预测误差协方差中的过程噪声项应为:

而不是直接使用 。

算法步骤

确定初始条件

Kalman 滤波需要给定初始状态估计和初始估计误差协方差:

其中:

  • :初始状态估计。
  • :初始状态估计误差协方差。
    如果初始状态已知得比较准确,则可以设置较小的初始协方差;如果初始状态不确定性较大,则应设置较大的协方差。

Kalman 滤波迭代过程

整个算法分为两个阶段:时间更新(预测)和测量更新(校正)。

  1. 时间更新(预测阶段)
    利用上一时刻的状态估计,预测当前时刻的状态。

(1)状态预测

该式表示:根据上一时刻的状态估计以及当前控制输入,预测当前时刻的状态。

(2)估计误差协方差预测

该式用于计算当前预测状态的不确定性。
2. 测量更新(校正阶段)
获得当前时刻的实际观测值 后,利用观测信息修正预测结果。

(1)计算观测残差

观测残差表示实际测量值与理论预测测量值之间的差异。

(2)计算残差协方差矩阵

(3)计算 Kalman 增益

Kalman 增益决定了观测残差对状态估计的修正程度。

(4)更新状态估计

(5)更新估计误差协方差

至此,第 时刻的 Kalman 滤波计算完成。

实际编程时,也可以使用 Joseph 形式更新协方差,以改善浮点运算中的数值稳定性。

编程实现

编写 Kalman 滤波函数

假设系统状态维数为 ,观测维数为 ,控制输入维数为 ,过程噪声维数为 。

为了兼容时变系统,下面使用三维数组存储各时刻的系统矩阵。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
function [x_filt, P_filt, K_hist] = KalmanFilter( ...
z, u, Phi, B, Gamma, H, Q, R, x0, P0)

% z : m × N 观测数据
% u : l × N 控制输入
% Phi : n × n × N 状态转移矩阵
% B : n × l × N 控制输入矩阵
% Gamma : n × q × N 过程噪声驱动矩阵
% H : m × n × N 观测矩阵
% Q : q × q × N 过程噪声协方差
% R : m × m × N 测量噪声协方差
% x0 : n × 1 初始状态估计
% P0 : n × n 初始估计误差协方差

N = size(z, 2);
n = length(x0);
m = size(z, 1);

% 初始化存储空间
x_filt = zeros(n, N+1);
P_filt = zeros(n, n, N+1);
K_hist = zeros(n, m, N);

% 设置初始条件
x_filt(:,1) = x0;
P_filt(:,:,1) = P0;

% Kalman 滤波递推
for k = 1:N

%% 1. 时间更新:状态预测

x_pred = Phi(:,:,k) * x_filt(:,k) ...
+ B(:,:,k) * u(:,k);

%% 2. 时间更新:协方差预测

P_pred = Phi(:,:,k) * P_filt(:,:,k) ...
* Phi(:,:,k)' ...
+ Gamma(:,:,k) * Q(:,:,k) ...
* Gamma(:,:,k)';

%% 3. 计算观测残差

e = z(:,k) - H(:,:,k) * x_pred;

%% 4. 计算残差协方差

S = H(:,:,k) * P_pred * H(:,:,k)' ...
+ R(:,:,k);

%% 5. 计算 Kalman 增益

K = P_pred * H(:,:,k)' / S;

%% 6. 状态估计更新

x_filt(:,k+1) = x_pred + K * e;

%% 7. 协方差更新(Joseph 形式)

I = eye(n);
A = I - K * H(:,:,k);

P_filt(:,:,k+1) = A * P_pred * A' ...
+ K * R(:,:,k) * K';

%% 8. 保存 Kalman 增益

K_hist(:,:,k) = K;

end

end

这里有几个需要注意的地方:

  1. x_filt(:,1) 表示初始状态估计,x_filt(:,k+1) 表示第 次观测更新后的状态估计。
  2. P_filt(:,:,k) 表示对应时刻的估计误差协方差。
  3. K_hist(:,:,k) 保存每次迭代的 Kalman 增益。
  4. 代码采用 Joseph 形式更新协方差矩阵,避免直接使用简化公式可能产生的数值对称性问题。

构造一个具体数值算例

为了验证上述算法,考虑一个二维运动系统:

其中, 表示位置, 表示速度。

假设采样时间为 ,采用匀速运动模型:

假设没有控制输入:

过程噪声作用矩阵取:

假设只能测量位置:

噪声协方差矩阵取:

初始条件设置为:

假设获得以下五组位置测量数据:

可以编写以下 MATLAB 测试程序:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
clc;
clear;

%% 1. 设置系统参数

T = 1;
N = 5;

Phi = [1 T; 0 1];
B = zeros(2,1);

Gamma = [T^2/2; T];

H = [1 0];

Q = 0.1;
R = 4;

%% 2. 初始条件

x0 = [0; 0];

P0 = [10 0;
0 10];

%% 3. 测量数据

z = [1 2.2 2.8 4.2 4.9];

u = zeros(1,N);

%% 4. 扩展为时变矩阵存储格式

Phi_all = repmat(Phi,1,1,N);
B_all = repmat(B,1,1,N);
Gamma_all = repmat(Gamma,1,1,N);
H_all = repmat(H,1,1,N);
Q_all = repmat(Q,1,1,N);
R_all = repmat(R,1,1,N);

%% 5. 执行 Kalman 滤波

[x_filt, P_filt, K_hist] = KalmanFilter( ...
z, u, Phi_all, B_all, Gamma_all, ...
H_all, Q_all, R_all, x0, P0);

%% 6. 输出滤波结果

disp('滤波后的状态估计:');

disp(x_filt(:,2:end));

disp('每次迭代的 Kalman 增益:');

disp(K_hist);

figure;

k = 1:N;

plot(k, z, 'ro', 'MarkerSize', 8);
hold on;

plot(k, x_filt(1,2:end), 'b-o', ...
'LineWidth', 1.5);

grid on;

xlabel('采样时刻 k');
ylabel('位置');

legend('原始测量值', 'Kalman滤波估计值');

title('Kalman滤波效果');

运行后,x_filt 的第一行表示位置估计,第二行表示速度估计,将原始测量数据与滤波后的估计结果绘制在同一张图中。

即使传感器只能测量位置,Kalman 滤波也能够利用系统的运动模型,对无法直接测量的速度进行估计。