1、理論部分

      上一篇介紹了線性卡爾曼濾波器，當系統為線性高斯模型時，濾波器能給出最優的估計，但是實際系統總是存在不同程度的非線性，如平方、三角關係、開方等。對於非線性系統，可以採用的一種方法是通過線性化方法將非線性系統轉換為近似的線性系統，即為EKF，核心思想是：圍繞濾波值將非線性函式展開成泰勒級數並略去二階及以上項，得到一個近似的線性化模型，然後應用卡爾曼濾波完成狀態估計。

系統方程

各個變數意義同上一節，其中f和h代表狀態和觀測的非線性函式。

在擴充套件卡爾曼濾波中，狀態的預測以及觀測值的預測由非線性函式計算得出，線性卡爾曼濾波中的狀態轉移矩陣A陣和觀測矩陣H陣由f和h函式的雅克比矩陣代替，假設狀態有n維，則求法如下：

有了上面矩陣的計算方法，EKF濾波過程同線性卡爾曼濾波相同，公式如下

2、實踐部分

        

根據圖中情景，選取橫向位置、速度，縱向位置、速度為狀態量，列出下面非線性狀態方程及觀測方程

根據狀態方程和觀測方程，計算雅克比矩陣如下

close all;
clear all;
%%  真實軌跡模擬
kx = .01;   ky = .05;       % 阻尼係數
g = 9.8;                    % 重力
t = 15;                     % 模擬時間
 
Ts = 0.1;                   % 取樣週期 
len = fix(t/Ts);            % 模擬步數
dax = 3; day = 3;       % 系統噪聲
X = zeros(len,4); 
X(1,:) = [0, 50, 500, 0]; % 狀態模擬的初值
for k=2:len
    x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4); 
    x = x + vx*Ts;
    vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
    y = y + vy*Ts;
    vy = vy + (ky*vy^2
 
-g+day*randn(1))*Ts;
    X(k,:) = [x, vx, y, vy];
end
%%  構造量測量
dr = 8;  dafa = 0.1;        % 量測噪聲
for k=1:len
    r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
    a = atan(X(k,1)/X(k,3))*57.3 + dafa*randn(1,1);
    Z(k,:) = [r, a];
end
%% ekf 濾波
Qk = diag([0; dax/10; 0; day/10])^2;
Rk = diag([dr; dafa])^2;
Pk = 10*eye(4);
Pkk_1 = 10*eye(4);
x_hat = [0,40,400,0]';
X_est = zeros(len,4);
x_forecast = zeros(4,1);
z = zeros(4,1);
for k=1:len
    % 1 狀態預測    
    x1 = x_hat(1) + x_hat(2)*Ts;
    vx1 = x_hat(2) + (-kx*x_hat(2)^2)*Ts;
    y1 = x_hat(3) + x_hat(4)*Ts;
    vy1 = x_hat(4) + (ky*x_hat(4)^2-g)*Ts;
    x_forecast = [x1; vx1; y1; vy1];        %預測值
    % 2  觀測預測
    r = sqrt(x1*x1+y1*y1);
    alpha = atan(x1/y1)*57.3;
    y_yuce = [r,alpha]';
    %  狀態矩陣
    vx = x_forecast(2);  vy = x_forecast(4);
    F = zeros(4,4);
    F(1,1) = 1;  F(1,2) = Ts;
    F(2,2) = 1-2*kx*vx*Ts;
    F(3,3) = 1;  F(3,4) = Ts;
    F(4,4) = 1+2*ky*vy*Ts;
    Pkk_1 = F*Pk*F'+Qk;
    % 觀測矩陣
    x = x_forecast(1); y = x_forecast(3);
    H = zeros(2,4);
    r = sqrt(x^2+y^2);  xy2 = 1+(x/y)^2;
    H(1,1) = x/r;  H(1,3) = y/r;
    H(2,1) = (1/y)/xy2;  H(2,3) = (-x/y^2)/xy2;
    Kk = Pkk_1*H'*(H*Pkk_1*H'+Rk)^-1;       %計算增益
    x_hat = x_forecast+Kk*(Z(k,:)'-y_yuce);      %校正
    Pk = (eye(4)-Kk*H)*Pkk_1;
    X_est(k,:) = x_hat;
end
%% 
figure, hold on, grid on;
plot(X(:,1),X(:,3),'-b');
plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180));
plot(X_est(:,1),X_est(:,3), 'r');
xlabel('X'); 
ylabel('Y'); 
title('EKF simulation');
legend('real', 'measurement', 'ekf estimated');
axis([-5,230,290,530]);