参考内容:书籍《卡尔曼滤波原理及应用------matlab仿真》这本书对kalman算法的解析很清晰,MATLAB程序很全,适合初学者(如有侵权,请联系删除(qq:1491967912))
本文主要内容是针对纯方位目标运动的分析。这类目标往往只有方位是唯一可靠的参数,因此利用所测得的目标方位角信息来估计目标的运动参数(位置、速度、加速度等)。
假设观测站对某匀速直线运动的目标进行纯方位跟踪,并且雷达站已经知道目标的初始状态。
1、建立模型
基于纯方位的目标运动模型可以写成下面的形式:
其中X(k)代表状态变量,F代表状态转移矩阵,U(k)代表运动过程受到的随机扰动(也就是噪声),G为噪声驱动矩阵。Z(k)为测量变量,V(k)是测量噪声。从上面的模型来看,状态方程X(k)是一个线性的,但观测方程Z(k)是非线性的。因此根据
扩展KALMAN滤波(EKF)基本知识和相应的MATLAB仿真中的非线性方程线性化的方法。得到相应的测量矩阵H。
再综合状态方程和测量方程,根据扩展KALMAN滤波(EKF)基本知识和相应的MATLAB仿真中1.2节给出的EKF递推方程,可以解决纯方位目标跟踪问题。
1.2 纯方位目标跟踪算法matlab程序。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %EKF在纯方位目标跟踪中的应用实例子 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc clear all close all T=1; %雷达扫描周期 N=120/T; %总的采样次数 X=zeros(4,N); %存放实际位置、速度信息 Z=zeros(1,N); %存放测量角度信息 X(:,1)=[200,10,300,5]; %初始化目标状态 x0=50; y0=100; Xstation=[50,100]; %雷达的位置 F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; %状态矩阵 G=[T^2/2 0;T 0;0 T^2/2;0 T]; %状态噪声矩阵 delta_w=1e-3; R=2; %测量噪声方差 Q=delta_w*diag([0.5 1]); %均方差 P=eye(4); %协方差矩阵初始化 for i=2:N X(:,i)=F*X(:,i-1)+G*sqrtm(Q)*randn(2,1); end for i=1:N Z(:,i)=ffun(X(:,i),Xstation)+sqrtm(R)*randn(1,1); end X_ekf=zeros(4,N); %EKF初始化。 X_ekf(:,1)=X(:,1); I=eye(4); for i=2:N Xn=F*X_ekf(:,i-1); Zn=ffun(Xn,Xstation); d=Dist(Xn,Xstation); dd=d^2; H=[(y0-Xn(3))/dd,0,(Xn(1)-x0)/dd,0]; P0=F*P*F'+G*Q*G'; Kk=P0*H'/(H*P0*H'+R); X_ekf(:,i)=Xn+Kk*(Z(i)-Zn); P=(I-Kk*H)*P0; end %误差分析 for i=1:N Err_KalmanFilter(i)=Dist(X(:,i),X_ekf(:,i)); end figure hold on;box on;grid on; plot(X(1,:),X(3,:),'-r*'); plot(X_ekf(1,:),X_ekf(3,:),'-b*'); legend('真实值','EKF估计值'); xlabel('横坐标X/m'); ylabel('纵坐标Y/m'); title('真实值和估计值结果图') figure grid on; plot(Err_KalmanFilter,'-ks','MarkerFace','r'); xlabel('时间\s'); ylabel('位置估计偏差/m'); title('估计值与真实值的偏差'); figure hold on;box on;grid on; plot(Z,'-ks','MarkerFace','r'); plot(Z+sqrtm(R)*randn(1,1),'-bo','MarkerFace','b'); legend('真实角度','加入噪声的角度'); xlabel('时间\s'); ylabel('角度值\°'); title('真实角度和引入噪声的角度比较');
function cita=ffun(X1,X0) if(X1(3,1)-X0(1,2)>=0) %y1-y0>0 if(X1(1,1)-X0(1,1)>0) %x1-x2>0 第一象限 cita=atand(abs((X1(3,1)-X0(1,2))/(X1(1,1)-X0(1,1)))); elseif(X1(1,1)-X0(1,1)==0) %y轴上半轴 cita=asind(1); else %x1-x3<0 第二象限 cita=90+atand(abs((X1(3,1)-X0(1,2))/(X1(1,1)-X0(1,1)))); end else if(X1(1,1)-X0(1,1)>0) %x1-x2>0 第四象限 cita=270+atand(abs((X1(3,1)-X0(1,2))/(X1(1,1)-X0(1,1)))); elseif(X1(1,1)-X0(1,1)==0) %y轴下半轴 cita=270; else %x1-x3<0 第三象限 cita=180+atand(abs((X1(3,1)-X0(1,2))/(X1(1,1)-X0(1,1)))); end end
function d=Dist(x1,x2) if length(x2)==2 d=sqrt((x1(1)-x2(1))^2+(x1(3)-x2(2))^2); else d=sqrt((x1(1)-x2(1))^2+(x1(3)-x2(3))^2); end end