【运动学】基于matlab Singer模型算法机动目标跟踪【含Matlab源码 1157期】

一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【运动学】基于matlab Singer模型算法机动目标跟踪【含Matlab源码 1157期】

获取代码方式2：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

备注：
订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

二、部分源代码

%**********利用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
function [x,y,zx,zy,NN]=target_movement
%函数定义：生成目标运动的真实值和测量值
% %***************仿真条件*******************************************************
T=2;      %雷达扫描周期
r=10000;  %量测误差方差
x0=2000;%目标在X轴方向的起始位置
y0=10000;%目标在Y轴方向的起始位置
xv0=0;%目标在X轴方向的起始速度
yv0=-15;%目标在Y轴方向的起始速度
NN=500;%采样点数
x=zeros(NN,1);%X轴位置初始化
y=zeros(NN,1);%Y轴位置初始化
x(1)=x0;%X轴初始位置
y(1)=y0;%Y轴初始位置
vx(1)=xv0;%X轴初始速度
vy(1)=yv0;%Y轴初始速度
for i=1:NN-1
    if i<200
        ax=0;
        ay=0;
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ay*T;
    elseif (i>=200)&(i<=300)
        ax=15/200;
        ay=15/200;
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ay*T;
    elseif (i>300)&(i<=500)
        ax=0;
        ay=0;
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ay*T;     
    end
    x(i+1)=x(i)+vx(i)*T+0.5*ax*T^2+0.5*0*T^2*randn;%X轴的动态方程
    y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2+0.5*0*T^2*randn;%Y轴的动态方程
end
%***************产生量测噪声********************
nx=100*randn(NN,1);
ny=100*randn(NN,1);
%***************量测值**************************
zx=x+nx;
zy=y+ny;
vvx=vx;
vvy=vy;
save target_movement_out vvx vvy

%i=1:NN;
%k=4:1:NN;
%l=4:1:NN;
%figure(1)
%plot(x,y,'-dm');
%title('目标运动轨迹')
%xlabel('x方向')
%ylabel('y方向')
%legend('目标运动轨迹')

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
  
34
  
35
  
36
  
37
  
38
  
39
  
40
  
41
  
42
  
43
  
44
  
45
  
46
  
47
  
48
  
49
  
50
  
51
  
52
  
53
  
54
  
55
  
56
  
57
  
58
  
59
  
60
  
61
  
62
  
63
  
64
  
65
  
66
  
67
  
68
  
69
  
70
  
71
  
72
  
73
  
74
  
75
  
76
  
77
  
78
  
79
  
80
  
81
  
82
  
83
  
84
  
85
  
86
  
87
  
88
  
89
  
90
  
91
  
92
  
93
  
94
  
95
  
96
  
97
  
98
  
99
  
100
  
101
  
102
  
103
  
104
  
105
  
106
  
107
  
108
  
109
  
110
  
111
  
112
  
113
  
114
  
115
  
116
  
117
  
118
  
119
  
120
  
121
  
122
  
123
  
124
  
125
  
126
  
127
  
128
  
129
  
130
  
131
  
132
  
133
  
134
  
135
  
136
  
137
  
138
  
139
  
140
  
141
  
142
  
143
  
144
 

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 门云阁.MATLAB物理计算与可视化[M].清华大学出版社，2013.

文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。

原文链接：qq912100926.blog.csdn.net/article/details/119081554