gusucode.com > 基于matlab编程Singer模型算法对机动目标进行跟踪程序源码 > 基于matlab编程Singer模型算法对机动目标进行跟踪程序源码/Singer/Singer.m
%**********利用Singer模型算法对机动目标进行跟踪*************
function [xx5,yy5,ex5,exv5]=singer(T,r,N)
clc
clear
close all
% %***************仿真条件********************
T=2; %雷达扫描周期
r=10000; %量测误差方差
N=50;%Monte Carlo仿真次数
%alpha=1/60;%机动时间常数的倒数，即机动频率
F=[1 T (1/2)*T^2 0 0 0;
0 1 T 0 0 0;
0 0 1 0 0 0;
0 0 0 1 T (1/2)*T^2;
0 0 0 0 1 T;
0 0 0 0 0 1];%状态转移矩阵
H=[1 0 0 0 0 0;
0 0 0 1 0 0];%量测矩阵
sigmax=r;%X方向量测噪声方差
sigmay=r;%Y方向量测噪声方差
R=[sigmax 0;
0 sigmay];%量测噪声协方差
%sigmaax=0.01;%X方向目标加速度方差
%sigmaay=0.01;%Y方向目标加速度方差
qq11=T^5/20;
qq12=T^4/8;
qq13=T^3/6;
qq22=T^3/3;
qq23=T^2/2;
qq33=T;
qq44=T^5/20;
qq45=T^4/8;
qq46=T^3/6;
qq55=T^3/3;
qq56=T^2/2;
qq66=T;
Q=[qq11 qq12 qq13 0 0 0;
qq12 qq22 qq23 0 0 0;
qq13 qq23 qq33 0 0 0;
0 0 0 qq44 qq45 qq46;
0 0 0 qq45 qq55 qq56;
0 0 0 qq46 qq56 qq66];%过程噪声协方差
for j=1:N
[x,y,zx,zy,NN]=target_movement;
load target_movement_out
z=[zx';zy'];
X=[z(1,3) (z(1,3)-z(1,2))/T (z(1,3)-2*z(1,2)+z(1,1))/T^2 z(2,3) (z(2,3)-z(2,2))/T (z(2,3)-2*z(2,2)+z(2,1))/T^2]';%状态向量初始化
%滤波协方差初始化
P11=R(1,1);
P12=R(1,1)/T;
P13=R(1,1)/T^2;
P22=2*R(1,1)/T^2;
P23=3*R(1,1)/T^3;
P33=6*R(1,1)/T^4;
P44=R(2,2);
P45=R(2,2)/T;
P46=R(2,2)/T^2;
P55=2*R(2,2)/T^2;
P56=3*R(2,2)/T^3;
P66=6*R(2,2)/T^4;
P=[P11 P12 P13 0 0 0;
P12 P22 P23 0 0 0;
P13 P23 P33 0 0 0;
0 0 0 P44 P45 P46;
0 0 0 P45 P55 P56;
0 0 0 P46 P56 P66];
MX(:,3)=X;
EX(j,3)=(X(1)-x(3)).^2;%x方向位置初始方差
EXv(j,3)=(X(2)-vvx(3)).^2;%x方向速度初始方差
EY(j,3)=(X(4)-y(3)).^2;%y方向位置初始方差
EYv(j,3)=(X(5)-vvy(3)).^2;%y方向速度初始方差
for i=4:NN
x1=F*X;
z1=H*x1;
P1=F*P*F'+Q;
S=H*P1*H'+R;
v=z(:,i)-z1;
W=P1*H'*inv(S);
X=x1+W*v;
P=P1-W*S*W';
Mv=v'*inv(S)*v;
MX(:,i)=X;
MEX(:,i,j)=MX(:,i);
EX(j,i)=(X(1)-x(i)).^2;%x方向位置初始方差
EXv(j,i)=(X(2)-vvx(i)).^2;%x方向速度初始方差
EY(j,i)=(X(4)-y(i)).^2;%y方向位置初始方差
EYv(j,i)=(X(5)-vvy(i)).^2;%x方向速度初始方差
end
end
EEX=sqrt(sum(EX)/N);
EEY=sqrt(sum(EY)/N);
exv5=sqrt(sum(EXv)/N);
eyv5=sqrt(sum(EYv)/N);
xx5=MX(1,:);
yy5=MX(4,:);
ex5=EEX;
ey5=EEY;
% ex5=EEY;
i=1:NN;
k=3:1:NN;
l=4:1:NN;
figure(1)
plot(x,y,'--r');
hold on
plot(zx,zy,'-b');
hold on
plot(xx5(k),yy5(k),'-g');
title('动目标跟踪Singer')
xlabel('x方向')
ylabel('y方向')
legend('目标运动真实轨迹','目标运动量测轨迹','跟踪轨迹Singer')
figure(2)
hold on
plot(k,EEX(k),'-r');
title('x方向位置协方差Singer')
xlabel('跟踪步数')
ylabel('x方向')
figure(3)
hold on
plot(k,EEY(k),'-r');
title('y方向位置协方差Singer')
xlabel('跟踪步数')
ylabel('y方向')
figure(4)
hold on
plot(k,exv5(k),'-r');
title('x方向速度协方差Singer')
xlabel('跟踪步数')
ylabel('x方向')
figure(5)
hold on
plot(k,eyv5(k),'-r');
title('y方向速度协方差Singer')
xlabel('跟踪步数')
ylabel('y方向')