testcv.m

在CV模型下的卡尔曼滤波算法MATLAB程序

x1_position=[];
x1radar_position=[];
x1_0=[7700,5600,9700,180,40,55,0,0,0]';               %运动轨迹仿真初始值1
[x1radar_position,x1_position,x1_state]=track_cv(x1_0,1.5,60,0.1);   %目标1在[0,90s]作CV运动
x2Dradar_position=x1radar_position(1:2,:);
[x2radar_position,x2_position,x2_state]=track_cv(x1_0,1.5,60,0.0001);
xIF_position=x2radar_position(2:3,:);
%卡尔曼滤波
x=[7700,5600,9700,180,40,55]';
P=1000*[eye(6)];%eye生成单位阵
F=[eye(3),diag([1.5 1.5 1.5]);
    zeros(3),eye(3)];
G=[diag([1.5 1.5 1.5]);
    eye(3)];
Cw=[diag([0.001^2 0.001^2 0.001^2])];
Cn=[10^2 0 0;0 0.05^2 0;0 0 0.05^2];
Cn1=[10^2 0;0 0.05^2];
Cn2=[0.005^2 0;0 0.005^2];
Fradar_position=[];

for i=1:60
    i
    x1=F*x;
    P=F*P*F'+G*Cw*G';
    H=[x1(1)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) x1(2)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) x1(3)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) 0 0 0;
        -x1(2)/(x1(1)^2+x1(2)^2) x1(1)/(x1(1)^2+x1(2)^2) 0 0 0 0];
    K=P*H'*inv(H*P*H'+Cn1);
    P=(eye(6)-K*H)*P;
    xtemp=x1+K*(x2Dradar_position(:,i)-[sqrt(x1(1)^2+x1(2)^2+x1(3)^2);atan(x1(2)/x1(1))]);
    H2=[-xtemp(2)/(xtemp(1)^2+xtemp(2)^2) xtemp(1)/(xtemp(1)^2+xtemp(2)^2) 0 0 0 0;
        -(xtemp(1)*xtemp(3))/(sqrt(xtemp(1)^2+xtemp(2)^2)*(xtemp(1)^2+xtemp(2)^2+xtemp(3)^2)) -(xtemp(2)*xtemp(3))/(sqrt(xtemp(1)^2+xtemp(2)^2)*(xtemp(1)^2+xtemp(2)^2+xtemp(3)^2)) sqrt(xtemp(1)^2+xtemp(2)^2)/(xtemp(1)^2+xtemp(2)^2+xtemp(3)^2) 0 0 0];
    K2=P*H2'*inv(H2*P*H2'+Cn2);
    x=xtemp+K2*(xIF_position(:,i)-[atan(xtemp(2)/xtemp(1));atan(xtemp(3)/sqrt(xtemp(1)^2+xtemp(2)^2))]);
    P=(eye(6)-K2*H2)*P;
    Fradar_position=[Fradar_position x];
end
q=[];
for i=1:60
    q(1,i)=x1radar_position(1,i)*cos(x1radar_position(2,i))*cos(x1radar_position(3,i));
    q(2,i)=x1radar_position(1,i)*cos(x1radar_position(2,i))*sin(x1radar_position(3,i));
    q(3,i)=x1radar_position(1,i)*sin(x1radar_position(2,i));
end
i=1:60;
plot(i,x1_position(1,i),'r',i,Fradar_position(1,i),'g',i,q(1,i),'b')