源程序:
clear
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%初始化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CV/CA模型要求给参数R、Q%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=50; % sample number N=50 points
t=0:0.1:5-0.1;
T=0.1; % sample time
v=0.5; % initial velocity
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CV初始化%%%%%%%%%%%%%%%%%%%%%%%%%%
y=zeros(2,1,50);
y(:,:,1)=[0;0.5];
for i=2:50;
y(1,i)=y(1,i-1)+y(2,i-1)*T+0.5*2*T.^2;
y(2,i)=y(2,i-1)+2*T;
end
h=zeros(2,1,50);
a=10*randn(2,1,50);
for i=1:50; % add white gauss noise
h(:,:,i)=y(:,:,i)+a(:,:,i); %data+noise
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CA初始化%%%%%%%%%%%%%%%%%%%%%%%
yy=zeros(3,1,50);
yy(:,:,1)=[0;0.5;2];
for i=2:50;
yy(1,i)=yy(1,i-1)+yy(2,i-1)*T+0.5*2*T.^2;
yy(2,i)=yy(2,i-1)+2*T;
yy(3,i)=yy(3,i-1);
end
hh=zeros(3,1,50);
a=10*randn(3,1,50);
for i=1:50; % add white gauss noise
hh(:,:,i)=yy(:,:,i)+a(:,:,i); %data+noise
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CV初始化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R=input('请输入测量噪声方差R的值: '); %%R=3
Q=input('请输入系统噪声方差Q的值: '); %%Q=3
H=[1 0;0 1];
B=[1/2*(T)^2 T]';
R=R*[1 0;0 1];
p=ones(2);
A=[1,T;
0,1];
U=[T.^2/2;
T];
c=[1 T T*T/2 ;
0 1 T ;
0 0 1]; %一步转移输矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CV卡尔曼滤波器主程序%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x=zeros(2,1,50);
for k=2:N;
x(:,:,k)=A*x(:,:,k-1);
p1=A*p*A'+B*Q*B';
K=p1*H'*inv(H*p1*H'+R);
x(:,:,k)=x(:,:,k-1)+K*(h(:,:,k)-H*x(:,:,k-1));
p=p1-K*H*p1;
end
p(1,1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CA初始化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R1=input('请输入测量噪声方差R的值: '); %R1=1
Q1=input('请输入系统噪声方差Q的值: '); %Q1=1
H=[1 0 0;0 1 0;0 0 1];
B1=[1/2*(T)^2 T 1]';
R1=R1*[1 0 0;0 1 0;0 0 1];
p=ones(3);
A1=[1,T,T.^2/2;
0,1,T;
0,0,1];
U1=[T.^2/2;
T;
1];
c=[1 T T*T/2 ;
0 1 T ;
0 0 1]; %一步转移输矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%CA卡尔曼滤波器主程序%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xx=zeros(3,1,50);
for k=2:N;
xx(:,:,k)=A1*xx(:,:,k-1);
p1=A1*p*A1'+B1*Q1*B1';
K=p1*H'*inv(H*p1*H'+R1);
xx(:,:,k)=xx(:,:,k-1)+K*(hh(:,:,k)-H*xx(:,:,k-1));
p=p1-K*H*p1;
end
p(1,1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画图%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
plot(t,x(1,:),'r',t,xx(1,:),'g',t,yy(1,:),'k-.');
figure(2)
plot(t,x(2,:),'r',t,xx(2,:),'g',t,yy(2,:),'k-.');
figure(3)
plot(t,xx(3,:),'g',t,hh(3,:),'b-',t,yy(3,:),'k-.');
评论0