# 反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');

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
评论
10-14
04-12
12-09 1451
02-11 1507
10-24 1万+
05-25
02-29 2939
08-17 2万+
11-21 5366

• 非常没帮助
• 没帮助
• 一般
• 有帮助
• 非常有帮助