《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.3

最新推荐文章于 2024-09-07 22:35:10 发布

小凡1991

最新推荐文章于 2024-09-07 22:35:10 发布

阅读量1.5k

点赞数 1

分类专栏：卡尔曼滤波原理

本文链接：https://blog.csdn.net/sinat_20265495/article/details/53306711

版权

卡尔曼滤波原理专栏收录该内容

16 篇文章 26 订阅

订阅专栏

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  扩展Kalman滤波在纯方位目标跟踪中的应用实例
%  详细原理介绍及中文注释请参考：
%  《卡尔曼滤波原理及应用-MATLAB仿真》，电子工业出版社，黄小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function EKF_angle
clc;clear;
T=1;
N=40/T;
X=zeros(4,N);
X(:,1)=[0,2,1400,-10];
Z=zeros(1,N);   % 此处有误，请修改为P91页一致即可运行
delta_w=1e-4;
Q=delta_w*diag([1,1]) ;
G=[T^2/2,0;T,0;0,T^2/2;0,T];
R=0.1*pi/180;
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
x0=0;
y0=1000; 
Xstation=[x0;y0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w=sqrtm(R)*randn(1,N);
for t=2:N
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);
end
for t=1:N
    Z(t)=hfun(X(:,t),Xstation)+w(t);
end
Xekf=zeros(4,N);
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(Xn,Xstation);
    D=Dist(Xn,Xstation);
    H=[-(Xn(3,1)-y0)/D,0,(Xn(1,1)-x0)/D,0];
    K=P1*H'*inv(H*P1*H'+R);
    Xekf(:,i)=Xn+K*(Z(:,i)-dd);
    P0=(eye(4)-K*H)*P1;
end
for i=1:N
  Err_KalmanFilter(i)=sqrt(Dist(X(:,i),Xekf(:,i)));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k.');
plot(Xekf(1,:),Xekf(3,:),'-r+');
legend('真实轨迹','EKF轨迹')
figure
hold on; box on;
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
figure 
hold on;box on;
plot(Z/pi*180,'-r.','MarkerFace','r');
plot(Z/pi*180+w/pi*180,'-ko','MarkerFace','g');
legend('真实角度','观测角度');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cita=hfun(X1,X0)
if X1(3,1)-X0(2,1)>=0
    if X1(1,1)-X0(1,1)>0
        cita=atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    elseif X1(1,1)-X0(1,1)==0
        cita=pi/2;
    else
        cita=pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    end
else
    if X1(1,1)-X0(1,1)>0
        cita=3*pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    elseif X1(1,1)-X0(1,1)==0
        cita=3*pi/2;
    else
        cita=pi+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    end
end
function d=Dist(X1,X2);
if length(X2)<=2
    d=( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
    d=( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

结果：