反q滤波 matlab,急求卡尔曼滤波器MATLAB程序,重谢...

clear

clc

t=0:0.1:(20-0.1);

T=0.1;            % sample time

v=0.5;            % initial velocity  初始速率

y=ones(1,200);

y(1)=0;

for n=2:200;

y(n)=y(n-1)+v*T;

end

figure(1);

plot(t,y,'-');

axis([0 20 0 20]);

xlabel('time');

ylabel('yp position');

title('the initial track of movement');

% add white gauss noise

a=0.5*randn(1,200);

s=y+a;            %data+noise

figure(2);

plot(t,s,'+');

axis([0 20 0 20]);

xlabel('time');

ylabel('yp position');

title('the track of movement with noise');

save initial_track s

load initial_track  s; % y:initial data,s:data with noise

T=0.1;

% yp denotes the sample value of position

% yv denotes the sample value of velocity

% Y=[yp(n);yv(n)];

% error deviation caused by the random acceleration

% known data

Y=zeros(2,200);

Y0=[0;1];

Y(:,1)=Y0;%初始状态

A=[1 T

0 1];

B=[1/2*(T)^2 T]';

H=[1 0];

C0=[0 0

0 1];

C=[C0 zeros(2,2*199)];

Q=(0.25)^2;

R=(0.25)^2;

% kalman algorithm ieration

for n=1:200

i=(n-1)*2+1;

K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);%卡尔曼增益矩阵

Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));%状态估计

Y(:,n+1)=A*Y(:,n);%状态预测

C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);%估计方差阵

C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';%进一步预测方差阵

end

% the diagram of position after filtering

figure(3)

t=0:0.1:20;

yp=Y(1,:);A

plot(t,yp,'+');

axis([0 20 0 20]);

xlabel('time');

ylabel('yp position');

title('the track after kalman filtering');

% the diagram of velocity after filtering

figure(4)

yv=Y(2,:);

plot(t,yv,'+');

xlabel('time');

ylabel('yv velocity');

title('the velocity caused by random acceleration');

  • 0
    点赞
  • 0
    收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:1024 设计师:我叫白小胖 返回首页
评论
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值