卡爾曼經典公式：

\[狀態一步預測： \qquad \hat{X}_{k/k-1} = \Phi_{k/k-1}\hat{X}_{k-1} \] \[狀態一步預測方差陣： \qquad P_{k/k-1}= \Phi_{k/k-1}P_{k-1}\Phi^T+\Gamma_{k-1}Q_{k-1}\Gamma^T_{k-1} \] \[濾波增益： \qquad K_k = P_{{XZ},k/k-1}P^{-1}_{ZZ,k/k-1}\\ 或者寫為:\qquad K_k = P_{k/k-1}H^T_k(H_kP_{k/k-1}H^T_k+R_k)^{-1} \] \[狀態估計：\qquad \hat{X} = \hat{X}_{k/k-1} + K_k(Z_k-H_k\hat{X}_{k/k-1}) \] \[狀態估計方差陣：\qquad P_k = (I- K_kH_k)P_{k/k-1} \]

舉例如下：

一維空間的卡爾曼濾波，主要參考嚴老師教材

\[X_k = 0.95 \cdot X_k-1+W_{k-1}\\ y_k = X_k + V_k \]

其中均值0，單位的高斯白噪聲

clear;
% 系統引數
Phik = 0.95; Bk = 1.0;Hk = 1.0;
% 噪聲引數，分別是標準差和方差
q =1;r = 3;Qk = q*q;Rk = r*r;

len =100;
%含有噪聲的標準正太分佈
w = q*randn(len,1);v = r*randn(len,1);
xk = zeros(len,1);yk = zeros(len,1);
xk(1) =r*randn(1,1);
for k = 2:len
    xk(k) = Phik*xk(k-1) + Bk*w(k);
    yk(k)= Hk*xk(k)+v(k);    
end

 % Kalman 濾波估計，初始狀態為0
 Xk = 0; 
%  第一步的誤差，這個數比較大，為很大的正實數？？？意義
 Pxk = 100*Rk/(Hk^2*Phik^2);

for k=1:len         
    [Xk, Pxk, Kk] = kalman(Phik, Bk, Qk, Xk, Pxk, Hk, Rk, yk(k));         
    res(k,:) = [Xk,Pxk,Kk];     
end
% 穩態濾波     
ss = [Hk^2*Phik^2  Hk^2*Bk^2*Qk+Rk-Phik^2*Rk  -Bk^2*Qk*Rk];     
Px = ( - ss(2) + sqrt(ss(2)^2-4*ss(1)*ss(3)) ) / (2*ss(1));     
K = Hk*(Phik^2*Px+Bk^2*Qk)/(Hk^2*(Phik^2*Px+Bk^2*Qk)+Rk);  
G = (1-K*Hk)*Phik;
% 這裡獲取真值
Xk_IIR = filter(K, [1 -G], yk);     
% 作圖     
subplot(1,2,1), hold off, plot(sqrt(res(:,2)),'-'), hold on, plot(res(:,3),'r:'); 
grid
xlabel('\itk'); ylabel('\it\surd P_x_k , K_k');

 subplot(122), hold off, plot(yk,'x'),
 hold on, 
 plot(xk,'m:'); 
 plot(res(:,1),'k'); 
 plot(Xk_IIR,'r-.'); 
 grid
 xlabel('\itk'); ylabel('\ity_k, x_k, x^\^_k, x^\^_k_,_I_I_R'); 

function [Xk, Pxk, Kk] = kalman(Phikk_1, Bk, Qk, Xk_1, Pxk_1, Hk, Rk, Yk)     
Xkk_1 = Phikk_1*Xk_1;
Pxkk_1 = Phikk_1*Pxk_1*Phikk_1' + Bk*Qk*Bk';

Pxykk_1 = Pxkk_1*Hk';
Pykk_1 = Hk*Pxykk_1 + Rk;

Kk = Pxykk_1*Pykk_1^-1;

Xk = Xkk_1 + Kk*(Yk-Hk*Xkk_1);
Pxk = Pxkk_1 - Kk*Pykk_1*Kk';
end
 

結果圖如下：

結果如下：

誤差和濾波增益很快收斂，濾波曲線存在滯後的情況，數字濾波器的相位延遲特點。