✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
目标跟踪技术普遍应用于军事与民用领域,而滤波算法是其重要组成部分,因此受到了广泛关注与研究.迭代扩展卡尔曼滤波(Iterated Extended Kalman Filter,IEKF)是一种性能较为优越的滤波算法,该算法结构简单,滤波精度较高,鲁棒性较强,本文对IEKF算法展开相应的研究.
⛄ 部分代码
% function to implement the Iterated Extended Kalman Filter (IEKF)
% Inputs:
% OBSn - the observations (with noise)
% xest - initial state space estimates
% Ouputs:
% Xp - predicted states
function Xp = f_IEKF(OBSn,xest)
load avar % r1,r2, L, and T
tol = .1; % tolerance for iterations
diff = 1;
count = 0;
F = [1 T 0 0 0 0 0;
0 1 0 0 0 0 0;
0 0 1 T 0 0 0;
0 0 0 1 0 0 0;
0 0 0 0 1 0 T;
0 0 0 0 0 1 T;
0 0 0 0 0 0 1]; % state transition matrix
n = size(OBSn,2); % number of observations
Xp = zeros(7,n); % make room
Pkp1 = 1e10*eye(7); %xest*xest'; %.1*ones(7,7);
%Pkp1 = xest*xest';
%Pkp1 = F*Pkp1*F';
%Pkp1 = (10*randn(7,1))*(10*randn(7,1)).';
%Pkp1 = F*Pkp1*F';
% for each observation
for i = 1:n;
% if this is the first iteration the prior predicted estimate is xest
% if this is not the first run the prior estimate is in Xp
if i == 1
xkm1 = xest;
else
xkm1 = Xp(:,i-1);
end
Pkm1 = Pkp1; % conditional covariance from last iteration
% iterations are started with the predicted estimate from the last run
xkn = xkm1;
while ~(diff < tol || count > 9)
count = count + 1;
H = [gradest(@(x)f_h1(x),xkn); gradest(@(x)f_h2(x),xkn)];
R = (.01*randn(2,1))*(.01*randn(2,1)).';
Rdiag = diag(R); R = diag(Rdiag);
K = Pkm1*H'*(H*Pkm1*H'+R)^-1;
xkn_temp = xkm1 + K*(OBSn(:,i)-f_h(xkn)-H*(xkm1-xkn));
diff = norm(abs(xkn_temp-xkn));
fprintf('diff = %g \n',diff)
xkn = xkn_temp;
end
H = [gradest(@(x)f_h1(x),xkn); gradest(@(x)f_h2(x),xkn)];
Pkk = (eye(7)-K*H)*Pkm1;
Pkp1 = F*Pkk*F';
Xp(:,i) = F*xkn;
clc;
fprintf('i = %g; Count is %g \n',i,count)
count = 0;
diff = 1;
end
⛄ 运行结果
⛄ 参考文献
[1]吴松伦. 基于迭代扩展卡尔曼滤波的目标跟踪算法研究[D]. 西北师范大学.
