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

Posted 2021-08-27 紫极神光

一、简介

目标跟踪是利用各种类型的传感器获得的关于目标信息，对目标的真实状态以及未来的状态进行估计和预测的一门技术。目标跟踪技术在军事以及民用等诸多领域里都有着广泛的应用。随着电子技术以及计算机技术的进步，各种新的技术与理论被应用到目标跟踪领域中来，目标跟踪技术也逐渐的发展成为一门跨学科，跨行业，多层面的技术。目标跟踪技术研究的主要内容是从传感器中获得的关于目标的不精确的信息中，准确的估计和预测到目标的真实信息，因此需要各种滤波器对收集到的数据进行滤波处理。卡尔曼（Kalman）滤波作为一种性能优良的滤波算法，在目标跟踪领域里得到广泛的应用，但是滤波器是以一定的目标跟踪的模型为基础的。因此目标跟踪的研究对象也主要包括跟踪模型和滤波算法这两个方面。在这两个方面，国内外的众多学者对此进行了深入的研究，并取得了较为丰硕的成果。作为一种新型的数据融合算法，交互式多模型（IMM）算法由于其优良的跟踪效果，较宽的跟踪频带，在最近几年得到了足够的重视

二、源代码

%**********利用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('目标运动轨迹')

三、运行结果

四、备注

版本：2014a