kalman_basedonstatistic.m

Kalman滤波的一个仿真程序

function y=Kalman_BasedOnStatistic()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kalman filter for maneuvering target track
% using "in current" statistical model
% 2007.6.1
% 姓名：黄石生 
% 学号：06020032
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
maxlen=900;  % 对应在画图中的最大采样点数,为方便分析实验结果，本程序增加了采样点数

v=300;     % 飞行器的速度

x0=-198000;  % 飞行器x坐标的位置

sigamx=100;  % 观测值的x方向的标准差
sigamy=100;  % 观测值的y方向的标准差

T=2;  % 采样间隔

len=abs(x0)/T/v      % 对应在第一段匀速直线运动过程中的采样数

for i=1:len
    x(i)=i*T*v+x0;
end

a=20;      % 转弯加速度
r=v^2/a;   % 由此确定的转弯半径

y0=r;  % 为方便起见，取半径即为y的坐标
y(1:len)=y0*ones(1,len);

w=a/v;   % 由此确定的角频率

time_circle=pi/w;  % 在圆弧形转弯中用的时间

len2=floor(time_circle/T);  % 对应在圆弧形弯道的采样点数

for i=1:len2
    temp=i*T*w;
    x(len+i)=r*sin(temp);
    y(len+i)=r*cos(temp); % 圆弧处的位置坐标
end

time_after_circle=T-(time_circle-len2*T);

for i=len+len2+1:maxlen
    x(i)=-(i-len-len2-1)*T*v-v*time_after_circle;
    y(i)=-y0;  % 第二段匀速直线运动过程的坐标
end

% 不含噪位置坐标，用于存储，方便后面分析结果  
xx=x;
yy=y;   
% 以上是仿真产生飞行轨迹
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 以下为Kalman滤波过程
% 由于对x,y是独立进行观测的，本程序分别对x,y坐标进行滤波
% 采用改进的“当前”统计模型
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 滤波初始状态变量
x_o=[x(3),(x(3)-x(2))/T,(x(3)+x(1)-2*x(2))/T]';
y_o=[y(3),(y(3)-y(2))/T,(y(3)+y(1)-2*y(2))/T]';

% 初始滤波误差方差矩阵
P0=zeros(3,3);
P0(1,1)=sigamx^2;
P0(1,2)=sigamx^2/T;
P0(2,1)=sigamx^2/T;
P0(2,2)=2*sigamx^2/T^2;
P0(3,3)=sigamx^2;

I=eye(3);

alfa=0.001; % 对应机动时间常数的倒数

% F为状态转移矩阵
F=[1,T,(alfa*T-1+exp(-alfa*T))/alfa^2;0,1,(1-exp(-alfa*T))/alfa;0,0,exp(-alfa*T)];

% 生成系统误差协方差矩阵Q
q11=(1-exp(-2*alfa*T)+2*alfa*T+2*alfa^3*T^3/3-2*alfa^2*T^2-4*alfa*T*exp(-alfa*T))/alfa^4;
q12=(exp(-2*alfa*T)+1-2*exp(-alfa*T)+2*alfa*T*exp(-alfa*T)-2*alfa*T+alfa^2*T^2)/alfa^3;
q13=(1-exp(-2*alfa*T)-2*alfa*T*exp(-alfa*T))/alfa^2;
q22=(4*exp(-alfa*T)-3-exp(-2*alfa*T)+2*alfa*T)/alfa^2;
q23=(exp(-2*alfa*T)+1-2*exp(-alfa*T))/alfa;
q33=1-exp(-2*alfa*T);
q=[q11,q12,q13;0,q22,q23;0,0,q33];

% 矩阵U
u1=(-T+alfa*T^2/2+(1-exp(-alfa*T))/alfa)/alfa;
u2=T-(1-exp(-alfa*T))/alfa;
u3=alfa*(1-exp(-alfa*T));

U0=[u1,u2,u3]';

montekaluo=1; % 蒙特卡洛仿真指标
maxsimu=100;  % 蒙特卡洛仿真次数
while (montekaluo<=maxsimu)

x=xx+sigamx*randn(1,maxlen);    
y=yy+sigamy*randn(1,maxlen);   % 分别给x,y方向的数据加上噪声

plot(x,y)
x_re=x; % x_re,y_re分别表示滤波后位置的估计
y_re=y;
        
sigam0=1; % 机动加速度方差初始值，在滤波过程中自适应调整
Q=sigam0^2*q; % 系统误差协方差矩阵
H=[1,0,0]; % 测量矩阵
R=sigamx^2; % 观测误差协方差矩阵
U=U0;

%%%%%%%%%%%%%%%%%%%%%%
% 对x坐标进行滤波
X=x_o; % 位置初值
P=P0; % 滤波方差初值
step=4;
while(step<=maxlen)
    a=X(3);  % 用滤波值作为机动加速的估计，进行自适应调整
    X=F*X+U*a; 
    P=F*P*F'+Q; % 状态预测
    S=H*P*H'+R; % 新息协方差 
    K=P*H'*inv(S); % 滤波增益
    Z=x(step); % 观测向量
    Innovationx(step-3)=Z-X(1); % 计算预测产生的信息
    X1=X+K*(Z-H*X); % 滤波
    P=(I-K*H)*P; % 调整滤波方差矩阵
    sigam=(4-pi)/pi*(a+2/T^2*(X1(2)-X(2)))^2;  % 对机动加速度的方差进行自适应调整
    Tempx(step-3)=sigam; % 存储机动加速度的方差，便于分析实验结果
    Q=sigam*q; % 调整机动加速度协方差矩阵
    X=X1;
    x_re(step)=X(1); % 滤波的位置分量
    vx_re(step-2)=X(2); % 滤波后的速度分量
    ax_re(step-2)=X(3); % 滤波后的加速度分量
    step=step+1;
end
%%%%%%%%%%%%%%%%%%%%
% 对y坐标进行滤波
% 各行代码的意义参见上述过程
Y=y_o;
P=P0;
R=sigamy^2;
step=4;
while(step<=maxlen)
    a=Y(3);
    Y=F*Y+U*a;
    P=F*P*F'+Q;
    S=H*P*H'+R;
    K=P*H'*inv(S);
    Z=y(step);
    Innovationy(step-3)=Z-Y(1);
    Y1=Y+K*(Z-H*Y);
    P=(I-K*H)*P;
    sigam=(4-pi)/pi*(a+2/T^2*(Y1(2)-Y(2)))^2;
    Tempy(step-3)=sigam;
    Q=sigam*q;
    Y=Y1;
    y_re(step)=Y(1);
    vy_re(step-2)=Y(2);
    ay_re(step-2)=Y(3);
    step=step+1;
end

%%统计各采样点产生的新息
Innox(montekaluo,:)=Innovationx;
Innoy(montekaluo,:)=Innovationy;

%% 统计各采样点位置滤波误差
error1(montekaluo,:)=xx-x_re;
error2(montekaluo,:)=yy-y_re;

%% 统计各采样点滤波后的速度分量
vx(montekaluo,:)=vx_re;
vy(montekaluo,:)=vy_re;

%% 统计各采样点滤波后的机动加速度分量
ax(montekaluo,:)=ax_re;
ay(montekaluo,:)=ay_re;

%% 统计各采样点机动加速度的方差
Tempxt(montekaluo,:)=Tempx;
Tempyt(montekaluo,:)=Tempy;


hold on
l1=plot(x_re,y_re,'r');
set(l1,'LineWidth',2);
title('Kalman滤波结果')

montekaluo=montekaluo+1;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算滤波过程中产生的新息
Innox_ave=sum(Innox)/maxsimu;
Innoy_ave=sum(Innoy)/maxsimu;

figure(2)
plot(1:maxlen-3,Innox_ave);
title('x方向新息');

figure(3)
plot(1:maxlen-3,Innoy_ave);
title('y方向新息');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算滤波后x,y坐标误差均值与方差
err_x=sum(error1)/maxsimu;
err_y=sum(error2)/maxsimu;

sigam_x=std(error1);
sigam_y=std(error2);

n=1:1:maxlen;
figure(4)
plot(n,err_x)
title('x方向误差的均值');

figure(5)
plot(n,err_y)
title('y方向误差的均值');

figure(6)
plot(n,sigam_x)
title('x方向误差的标准差');

figure(7)
plot(n,sigam_y)
title('y方向误差的标准差');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算x,y方向各采样点的速度与加速度
vx_ave=sum(vx)/maxsimu;
vy_ave=sum(vy)/maxsimu;

ax_ave=sum(ax)/maxsimu;
ay_ave=sum(ay)/maxsimu;

figure(8)
plot(1:maxlen-2,vx_ave)
title('x方向的速度');

figure(9)
plot(1:maxlen-2,vy_ave)
title('y方向的速度');

figure(10)
plot(1:maxlen-2,ax_ave)
title('x方向的加速度');

figure(11)
plot(1:maxlen-2,ay_ave)
title('y方向的加速度');



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算x,y方向各采样点的机动加速度的方差
Tempx_ave=sum(Tempxt)/maxsimu;
Tempy_ave=sum(Tempyt)/maxsimu;

figure(12)
plot(1:maxlen-3,Tempx_ave);
title('x方向机动加速度的方差');

figure(13)
plot(1:maxlen-3,Tempy_ave);
title('y方向机动加速度的方差');