clear all; clc;
sigma=10000;
T=2;
t=300;
Vx=15;
H=[1 0 0;0 1 0];
Q=[1 0 0;0 1 T;0 0 1];
eXk(:,t)=[0 0 0]';eXz(:,t)=[0 0 0]';eeXz(:,t)=[0 0]';
N=10;%蒙特卡洛次数
for i=1:N
for j=1:t
Zk(:,j)=[2000+wgn(1,1,40);-10000+Vx*T*(j-1)+wgn(1,1,40)];
end
for j=1:300
if j==1
Xk(:,1)=[Zk(1,1),Zk(2,1),0]';Xk1(:,1)=Xk(:,1);
Xk(:,2)=[Zk(1,2),Zk(2,2),(Zk(2,2)-Zk(2,1))/T]';Xk1(:,2)=Xk(:,2); Pk=[sigma,0,0;0,sigma,sigma/T;0,sigma/T,2*sigma/T];
else
if j>2
Xk1(:,j)=Q*Xk(:,j-1);%预测
Pk1=Q*Pk*Q';%预测误差协方差
Kk=Pk1*H'*inv(H*Pk1*H'+sigma*eye(2));%kalman增益
Xk(:,j)=Xk1(:,j)+Kk*(Zk(:,j)-H*Xk1(:,j));%滤波
Pk=(eye(3)-Kk*H)*Pk1;%滤波协方差
end
end
%%
%1000次求平均
eXk(:,j)=eXk(:,j)+Xk(:,j)/N;%滤波
eXz(:,j)=eXz(:,j)+([2000;-10000+Vx*(j-1)*T;0]-Xk(:,j))/N;%滤波误差均值
本内容试读结束,登录后可阅读更多
下载后可阅读完整内容,剩余2页未读,立即下载
- 1
- 2
前往页