卡爾曼濾波 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

仿真輸出