1、理論部分
系統方程
各個變數意義同上一節，其中f和h代表狀態和觀測的非線性函式。
在擴充套件卡爾曼濾波中，狀態的**以及觀測值的**由非線性函式計算得出，線性卡爾曼濾波中的狀態轉移矩陣a陣和觀測矩陣h陣由f和h函式的雅克比矩陣代替，假設狀態有n維，則求法如下：
有了上面矩陣的計算方法，ekf濾波過程同線性卡爾曼濾波相同，公式如下
2、實踐部分
根據圖中情景，選取橫向位置、速度，縱向位置、速度為狀態量，列出下面非線性狀態方程及觀測方程
根據狀態方程和觀測方程，計算雅克比矩陣如下
%% 真實軌跡模擬
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; **** = 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 + *****randn(1,1);
z(k,:) = [r, a];
end%% ekf 濾波
qk = diag([0; dax/10; 0; day/10])^2;
rk = diag([dr; ****])^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]);
**結果
擴充套件卡爾曼濾波 不變擴充套件卡爾曼濾波 技術報告
本文使用 zhihu on vscode 創作並發布 connor 2020.06.02 不變擴充套件卡爾曼濾波的適用範圍 不變擴充套件卡爾曼濾波的使用步驟 乙個矩陣李群 matrix lie group g是滿足下列條件的 其中 表示 維單位矩陣，表示矩陣 的逆，表示矩陣乘法。現在，將上述構成李群...
初學者的卡爾曼濾波 擴充套件卡爾曼濾波
簡介 已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被發表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著重於將卡爾曼濾波應用於非線性系統。為什麼學界要這麼熱衷於將卡爾曼濾波器用於非線性系統呢？因為卡爾曼濾波器從一開始就是為...
初學者的卡爾曼濾波 擴充套件卡爾曼濾波（一）
簡介 已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被發表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著重於將卡爾曼濾波應用於非線性系統。為什麼學界要這麼熱衷於將卡爾曼濾波器用於非線性系統呢？因為卡爾曼濾波器從一開始就是為...