www.pudn.com > adpmov.rar > adpmov.m

clear all 
clc 
N=70; 
t=0:0.5:35-0.5; 
T=0.5; 
a=0; %加速度 
v=0.1; %初速度 
y=ones(1,N); 
y(1)=0; %初始位置 
for n=2:N; 
    y(n)=y(1)+v*(n-1)*T+1/2*a*((n-1)*T)^2; 
end 
figure(1); 
plot(t,y,'*'); 
xlabel('时间'); 
ylabel('位置'); 
title('理想运动时位置曲线'); 
 
% 加白噪声 
a=3*randn(1,N); 
s=y+a; 
figure(2); 
plot(t,s,'*'); 
xlabel('时间'); 
ylabel('位置'); 
title('加白噪声后的位置曲线'); 
 
Y=zeros(2,N); 
Y(:,1)=[0;0.2;0.1]; 
A=[1 T;0 1];   
H=[1 0 ]; 
C0=[0 0;0 1]; 
C=[C0 zeros(2,2*N-1)]; 
B=[T 1]'; 
% B=[1 1 1]' 
Q=(0.25)^2;  
R=(0.25)^2;  
 
% 卡尔曼算法 
 
for n=1:N 
    i=(n-1)*2+1; 
    C(:,i:i+1)=A*C(:,i:i+1)*A'+B*Q*B'; 
    K=C(:,i:i+2)*H'*inv(H*C(:,i:i+2)*H'+R); 
    C(:,i+3:i+5)=(eye(3)-K*H)*C(:,i:i+2); 
    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n)); 
    Y(:,n+1)=A*Y(:,n);  
end 
 
figure(3) %位置的跟踪曲线 
for n=1:N 
yp(n)=Y(1,n); 
end 
plot(t,yp,'*'); 
xlabel('时间'); 
ylabel('位置'); 
title('位置跟踪曲线'); 
 
figure(4) %速度的跟踪曲线 
for n=1:N 
yv(n)=Y(2,n); 
end 
plot(t,yv,'*'); 
xlabel('时间'); 
ylabel('速度'); 
title('速度跟踪曲线'); 
 
figure(5) %加速度的跟踪曲线 
for n=1:N 
ya(n)=Y(3,n); 
end 
plot(t,ya,'*'); 
xlabel('时间'); 
ylabel('加速度'); 
title('加速度跟踪曲线');