jiyutongji.m

基于统计模型的滤波方法

function y=jiyutongji()
% complement the procedure of kalman filter
%2007.5.3
%jason

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; % 不含噪位置坐标
    
% x=xx+sigamx*randn(1,maxlen);    
% y=yy+sigamy*randn(1,maxlen);   % 分别给x,y方向的数据加上噪声
% 
% plot(x,y)
% x_re=x;
% y_re=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.01;
F=[1,T,(alfa*T-1+exp(-alfa*T))/alfa^2;0,1,(1-exp(-alfa*T))/alfa;0,0,exp(-alfa*T)];

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];

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=10;
while (montekaluo<=maxsimu)

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

plot(x,y)
x_re=x;
y_re=y;
        
sigam0=1;
Q=sigam0^2*q;
H=[1,0,0];
R=sigamx^2;
U=U0;

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);
    X1=X+K*(Z-H*X);
    P=(I-K*H)*P;
    sigam=(X1(3)-X(3))^2;
%     sigam=(4-pi)/pi*(a+2/T^2*(X1(2)-X(2)))^2;
%     sigam=0;
    Q=sigam*q;
    X=X1;
    x_re(step)=X(1);
    step=step+1
end

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);
    Y1=Y+K*(Z-H*Y);
    P=(I-K*H)*P;
    sigam=(Y1(3)-Y(3))^2;
%     sigam=(4-pi)/pi*(a+2/T^2*(Y1(2)-Y(2)))^2;
%     sigam=0;
    Q=sigam*q;
    Y=Y1;
    y_re(step)=Y(1);
    step=step+1
end

error1(montekaluo,:)=xx-x_re;
error2(montekaluo,:)=yy-y_re;

% error1(montekaluo,:)=xx-x;
% error2(montekaluo,:)=yy-y;

hold on
plot(x_re,y_re,'r');

montekaluo=montekaluo+1;
end

err_x=sum(error1)/maxsimu;
err_y=sum(error2)/maxsimu;

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

n=1:1:maxlen;
figure(2)
plot(n,err_x)
title('error of x');

figure(3)
plot(n,err_y)
title('error of y');

figure(4)
plot(n,sigam_x)
title('std of x');

figure(5)
plot(n,sigam_y)
title('std of y');