trace3.asv

本程序是基于机动目标跟踪课题的整个算法程序

function main()
%产生观测数据
total=3*60;%总的时间长度
global T;%采样周期
T=1;
N=total/T;%数据长度
a=50;
var_rx=100;
var_ry=100;

X=[];%观测数据
X_ideal=[];%理想数据

for i=1:N
    [rx,ry]=track(i*T,20);
    X_ideal=[X_ideal,[rx;ry]];
    rx=rx+var_rx*randn(1,1);
    ry=ry+var_ry*randn(1,1);
    X=[X,[rx;ry]];
end

X_filter=zeros(size(X));%滤波后数据
X_mean=X_filter;%蒙特卡洛平均数据
Error_var=zeros(size(X));
M=10;%蒙特卡洛仿真次数

for iCount=1:M
    X_filter=Trace(X);
    X_mean=X_mean+X_filter;
    Error_var=Error_var+(X_ideal-X_filter).^2;
    
end

X_mean=X_mean/M;
Error_var=Error_var/M;
Error_mean=X_ideal-X_mean;%误差均值
Error_var=sqrt(Error_var-Error_mean.^2);

plot(X(1,:),X(2,:),X_mean(1,:),X_mean(2,:));
axis equal;
legend('真实轨迹','滤波轨迹');

figure;
k=1:N;
subplot(2,1,1),plot(k,Error_var(1,:)/N);title('x方向误差标准值');xlabel('采样次数'),ylabel('误差标准值(米)');
subplot(2,1,2),plot(k,Error_var(2,:)/N);title('y方向误差标准值');xlabel('采样次数'),ylabel('误差标准值(米)');
    
%理想航迹方程
function [x,y]=track(t,a)
%   t:时间
%   x:横轴位移
%   y:纵轴位移
%   a:转弯处加速度 
%   r:初始位置
%   v:初始速度

r=[-20000,0]';
v=300+randn(1,1);
w=a/v;%角速度
t1=-r(1)/v;
t2=t1+pi/w;
D=v^2/a*2;%圆周运动直径
if t<=0
   x=-20000,y=0;
elseif t>0&&t<=t1
    x=r(1)+v*t;
    y=r(2);
elseif t>t1&&t<=t2
    angel=(t-t1)*w;
    x=D/2*sin(angel);
    y=-D*(sin(angel/2))^2;
else
    x=-v*(t-t2);
    y=-D;
end

function R=Trace(X)
%飞行器跟踪模拟
%    X:观测数据
%    R:输出坐标

%观测时间间隔
global T;

%观测矩阵
H=[1,0,0,0,0;...
   0,1,0,0,0];

%位移测量误差
var_rx=100;
var_ry=100;
var_rx2=var_rx^2;
var_ry2=var_ry^2;

%观测噪声协方差矩阵
C=[var_rx2,0;...
   0,var_ry2];

%状态噪声协方差矩阵
var_v=30;
var_a=5;
var_v2=var_v^2;
var_a2=var_a^2;

Q=zeros(5,5);
Q(4,4)=var_v2;
Q(5,5)=var_a2;

%初始状态
s0=[-20000,0,0,300,0]';
%Kalman滤波跟踪
N=size(X,2);%观测数据长度
s=s0;
a=@traverse;
M=Q;
Xplus=[];%修正后的航迹
for icurrent=1:N
    [s,M]=Karlman(s,M,X(:,icurrent),a,Q,C,H);
    Xplus=[Xplus;(s(1:2))'];
end

R=Xplus';

function s_estimate=traverse(s)
%状态方程
global T;
s_estimate=zeros(5,1);
s_estimate(1)=s(1)+s(4)*cos(s(3))*T;
s_estimate(2)=s(2)-s(4)*sin(s(3))*T;
s_estimate(3)=s(3)+(s(5)/s(4))*T;
s_estimate(4)=s(4);
s_estimate(5)=s(5);

function [s,M]=Karlman(s_forward,M_forward,X,a,Q,C,H)
%卡尔曼滤波
%参数说明
%       X--观测数据矢量
%       A--状态矩阵

%       Q--状态噪声协方差
%       C--观测噪声协方差
%       h--观测方程句柄
%       s--输出数据矢量
%       s_foward--前次输出矢量
%       M--前次预测矩阵
global T;
%预测
s=feval(a,s_forward);  
%状态转换矩阵
A=[1,0,-s(4)*sin(s(3))*T,cos(s(3))*T,0;...
   0,1,-s(4)*cos(s(3))*T,-sin(s(3))*T,0;...
   0,0,1,-s(5)*T/(s(4))^2,T/s(4);...
   0,0,0,1,0;...
   0,0,0,0,1];
%最小预测MSE矩阵
M=M_forward;
M=A*M*A'+Q;   %协方差的进一步预测
%卡尔曼增益矩阵
K=M*H'*inv(C+H*M*H');
%修正（状态更新方程）
s=s+K*(X-H*s);
%最小MSE矩阵（协方差更新方程）
I=eye(5);
M=[I-K*H]*M*[I+K*H]'-K*C*K';

%粒子滤波
N1=100;% 粒子滤波器粒子数
P=2;
r=[-20000,0]';
xhat = r;
xhatPart = r;
% 初始化粒子过滤器
for i = 1 : N1
    rpart(i) = r + sqrt(P) * randn;
end
for k = 1 : N1
    % 系统仿真
    for i=1:N
        t=i*T;
        v=300+randn(1,1);
w=a/v;%角速度
   t1=-r(1)/v;
   t2=t1+pi/w;
if  t>0&&t<=t1
    x=r(1)+v*t;%状态方程
elseif t>t1&&t<=t2
    angel=(t-t1)*w;
    x=D/2*sin(angel);%状态方程
else
    x=-v*(t-t2);%状态方程
    
end
 y = x + sqrt(1) * randn;%观测方程
 for i = 1 : N
     if  t>0&&t<=t1
        xpartminus(i) = r(1)+v*t + sqrt(1) * randn;
        elseif t>t1&&t<=t2
             xpartminus(i)=D/2*sin(angel);
             else
                xpartminus(i)=-v*(t-t2);
        ypart = xpartminus(i);
        vhat = y - ypart;%观测和预测的差
        q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
     end
    end
    %正常化的可能性，每个先验估计
    qsum = sum(q);
    for i = 1 : N
        q(i) = q(i) / qsum;%归一化权重
    end
    % 重采样
    for i = 1 : N1
        u = rand; % 均匀随机数介于0和1
        qtempsum = 0;
        for j = 1 : N1
            qtempsum = qtempsum + q(j);
            if qtempsum >= u
                xpart(i) = xpartminus(j);
                break;
            end
        end
    end
    xhatPart = mean(xpart);
    xhatPartArr = [xhatPartArr xhatPart];
   t = 0 : N;
end

figure;
plot(t, xhatPartArr, 'r');