【目标跟踪】基于迭代扩展卡尔曼滤波器 (IEKF) 进行对象跟踪附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]. 西北师范大学.

⛄ Matlab代码关注

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料