擴充套件卡爾曼濾波器的實驗說明：

摘要，僅對非線性函式的卡爾曼濾波進行處理，效果圖如下：

在使用的過程中，只需要將狀態方程進行線性化處理即可，但是當濾波誤差和一步預測誤差較大的時候無法使用，也

即是過程噪聲太大的時候，誤差變大。

程式程式碼

clc;
clear;
T = 50;
Q= 0.1;
R =1;
w = sqrt(Q)*randn(1,T);
v= sqrt(R)*randn(1,T);

x = zeros(1,T);
x(1) = 0.1;
y= zeros(1,T);
y(1) = x(1)^2/20+v(1);
for k = 2:T
    x(k)= 0.5*x(k-1)+2.5*x(k-1)/(1+x(k-1)^2)+8*cos(1.2*k)+w(k-1);
    y(k) = x(k)^2/20+v(k);
end

% 進行濾波演算法的設計
X_ekf = zeros(1,T);
X_ekf(1)=x(1);
Y_ekf = zeros(1,T);
Y_ekf(1) = y(1);
P = zeros(1,T);
P(1) = 10;
for k =2:T
    Xn =  0.5*X_ekf(k-1)+2.5*X_ekf(k-1)/(1+X_ekf(k-1)^2)+8*cos(1.2*k);
    Zn = Xn^2/20;
    Phi = 0.5 +2.5*(1-Xn^2)/(1+Xn^2)^2;
    H = Xn/10;
    P_t = Phi*P(k-1)*Phi'+ Q;
    Kk = P_t*H'*inv(H*P_t*H'+R);
    X_ekf(k) = Xn+Kk*(y(k)-Zn);
    P(k) = (eye(1)-Kk*H)*P_t;    
end
X_std = zeros(1,T);
for k =1:T
    X_std(k) = abs(X_ekf(k)-x(k));
end

figure
hold on;
box on;
plot(x,'-ko','MarkerFaceColor','g');
plot(X_ekf,'-ks','MarkerFaceColor','b');
legend('真實值','kalman 濾波值');
xlabel('時間/s');
ylabel('狀態值 x');

figure 
hold on;box on;
plot(X_std,'-ko','MarkerFaceColor','g');
xlabel('時間/s');
ylabel('狀態估計偏差');

figure
hold on;box on;
plot(P);
 

程式輸出結果：

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片儲存下來直接上傳(img-ZUCA8f6G-1649235347130)(E:\Icodetest\forMatlab\Kalman\ekf_img\untitled2.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片儲存下來直接上傳(img-yJkuKWmR-1649235347130)(E:\Icodetest\forMatlab\Kalman\ekf_img\untitled1.png)]

Px的協方差濾波

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片儲存下來直接上傳(img-eMgTHHoV-1649235347131)(E:\Icodetest\forMatlab\Kalman\ekf_img\untitled3.png)]