卡尔曼滤波 Kalman Filter 理解 (超简单),带仿真

卡尔曼滤波 Kalman Filter 精简理解

参考外文 An Introduction to the Kalman Filter

核心过程一 （时间更新：）

初始输入（k-1时刻）：系统最优状态、系统噪声协方差
系统状态转移方程（1.9）
过程噪声协方差 （1.10）

核心过程二 （测量更新：）

计算卡尔曼增益K （1.11）
根据卡尔曼增益K，计算最优当前状态 （1.12）
根据卡尔曼增益K，计算当前系统噪声协方差 （1.13）

图表化整个循环迭代过程

MATLAB代码

%
%   ---------------------------------------------------------------------
%
%   Copyright 2019 wmx qq 843230304
%
%   ---------------------------------------------------------------------
%
function y = kalman01(z) 
%-------初始化-----------------------------
% 初始化 状态转移系数矩阵A
dt=1;
A=[ 1 0 dt 0 0 0;...
    0 1 0 dt 0 0;...
    0 0 1 0 dt 0;...
    0 0 0 1 0 dt;...
    0 0 0 0 1 0 ;...
    0 0 0 0 0 1 ];

% Measurement matrix
% 系统测量系数矩阵
H = [ 1 0 0 0 0 0; 0 1 0 0 0 0 ];

% 状态转移过程噪声
Q = eye(6);

% 测量噪声
R = 1000 * eye(2);

% Initial conditions
% 系统状态和协方差初始条件
persistent x_est p_est
if isempty(x_est)
    x_est = zeros(6, 1);
    p_est = zeros(6, 6);
end

%----------------Predicted state and covariance-------------------------
% 根据状态转移方程 预测理论值(预测状态 预测协方差)
x_prd = A * x_est;
p_prd = A * p_est * A' + Q;

% ------------------- Estimation --------------------------------------------
% 最优估计(根据理论预测和测量,使误差最小)
% 计算卡尔曼增益klm_gain
S = H * p_prd' * H' + R;
B = H * p_prd';
klm_gain = (S \ B)';

% Estimated state and covariance
% 根据卡尔曼增益klm_gain,
% 最优估计系统 当前状态x_est  当前协方差p_est, 提供下次迭代计算使用
x_est = x_prd + klm_gain * (z - H * x_prd);
p_est = p_prd - klm_gain * H * p_prd;

% Compute the estimated measurements
% 卡尔曼滤波输出
y = H * x_est;
end

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

仿真输出