EKF 方位跟踪算法

非线性KLman对于方位目标跟踪算法的MTALB程序实验zzzyy

function EKF_angle
clc;clear;
T=1;
N=40/T;
X=zeros（4N）;
X（:1）=[021400-10];
Z=zeros（1N）;   % 此处有误，请修改为P91页一致即可运行
delta_w=1e-4;
Q=delta_w*diag（[11]） ;
G=[T^2/20;T0;0T^2/2;0T];
R=0.1*pi/180;
F=[1T00;0100;001T;0001];
x0=0;
y0=1000; 
Xstation=[x0;y0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w=sqrtm（R）*randn（1N）;
for t=2:N
    X（:t）=F*X（:t-1）+G*sqrtm（Q）*randn（21）;
end
for t=1:N
    Z（t）=hfun（X（:t）Xstation）+w（t）;
end
Xekf=zeros（4N）;
Xekf（:1）=X（:1）;
P0=eye（4）;
for i=2:N
    Xn=F*Xekf（:i-1）;
    P1=F*P0*F‘+G*Q*G‘;
    dd=hfun（XnXstation）;
    D=Dist（XnXstation）;
    H=[-（Xn（31）-y0）/D0（Xn（11）-x0）/D0];
    K=P1*H‘*inv（H*P1*H‘+R）;
    Xekf（:i）=Xn+K*（Z（:i）-dd）;
    P0=（eye（4）-K*H）*P1;
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        2122  2018-01-11 14:35  4.3.5\EKF_angle.m
     目录           0  2018-03-14 21:20  4.3.5\