white_noise.m

用于机动目标跟踪的白噪声模型算法

% function []=white_noise(T)
%%%定义可调白噪声模型函数
% clear all
% clc
%%%初始值确定
%%%系统的状态向量表示为X(k)=[x x' y y']'
% x0=[z12 (z12-z11)/T z22 (z22-z21)/T];       %%%初始值的确定；zij中i=1表示x方向的量测值，i=2表示y方向的量测值，j=k表示k时刻的量测数据
X0=[120000 -426 2000 0]';
r=sqrt(120426^2+2000^2);
a=0.0166;
d=100;
e=0.03;
T=1;
A=[cos(a) -r*sin(a)
   sin(a) r*cos(a)];                        %%%a表示方位角θ,r表示径向距离
B=[d 0
   0 e];                                    %%%d代表径向距离测量误差的协方差，e代表方位角测量误差的协方差
R=A*B*A';                                   %%%量测噪声在直角坐标系下的协方差
P_yuce=[R(1,1) R(1,1)/T R(1,2) R(1,2)/T
        R(1,1)/T 2*R(1,1)/T^2 R(1,2)/T 2*R(1,2)/T^2
        R(1,2) R(1,2)/T R(2,2) R(2,2)/T
        R(1,2)/T 2*R(1,2)/T^2 R(2,2)/T 2*R(2,2)/T^2];   %%%初始协方差确定

      
%%%初始条件确定
F=[1 T 0 0
   0 1 0 0
   0 0 1 T
   0 0 0 1];                                       %%%一步转移矩阵
C=[1 0 0 0
   0 0 1 0];                                       %%%量测矩阵
% R=                                               %%%R值在求P_chushi值的时候已经确定，为一2*2的对角矩阵
% Q_0=[T^5/20 T^4/8 0 0
%    T^4/8 T^3/3 0 0
%    0 0 T^5/20 T^4/8
%    0 0 T^4/8 T^3/3];                              %%%量测噪声序列的协方差矩阵
Q_0=500*eye(4,4);
I=eye(4,4);
W1=100*randn(1,90);
W2=100*randn(1,90);
          
%%%算法
Q=Q_0;
X_yuce(:,1)=X0;
Y_max=5.07;
c=1e8;
N=90;
for k=1:N
    [x,y]=surrounding2;                       
    Z(:,k)=[x(k) y(k)]'+[W1(k) W2(k)]';        %%%此处调用函数，引入z(k)
    X_guji(:,k)=F*X_yuce(:,k);                 
    V=Z(:,k)-C*X_guji(:,k);                    %%%V表示新息
    P_guji=F*P_yuce*F'+Q;                      
    S=C*P_guji*C'+R;                           %%%S代表新息过程协方差矩阵
    Y=V'*inv(S)*V;                             
    if(Y>Y_max)                                %%%Y_max表示机动发生的判决门限值
        Q=c*Q;                                 %%%c为一常数
    else
        Q=Q_0;
    end
%   P_guji=F*P_yuce*F'+Q;
    K=P_guji*C'*inv(S);
    X_yuce(:,k+1)=X_guji(:,k)+K*V;
    P_yuce=(I-K*C)*P_guji;
end

plot(X_yuce(1,:),X_yuce(3,:))
hold on;
plot(Z(1,:),Z(2,:),'r')