- 积分
- 0
- 注册时间
- 2012-10-31
- 仿真币
-
- 最后登录
- 1970-1-1
|
小弟刚开始看卡尔曼滤波这一块。最近做一个物体轨迹的估计。看了[基于卡尔曼滤波预测的移动机器人矢量场矩形法避障研究]这片论文后仿照其中的卡尔曼滤波一章中的系统方程和观测方程,自己写了下面的程序。
clear
%假设物体从(0,10)位置开始X方向做速度为1的匀速运动,Y方向做初速为0加速度
%为0.16的加速度运动,T=0.2秒采样一次%
a=[1,0.2,0,0;0,1,0,0;0,0,1,0.2;0,0,0,1];
b=[0.02,0,0,0;0,0.2,0,0;0,0,0.02,0;0,0,0,0.2];
Ax=0;
Ay=-0.16;
A=[Ax;Ax;Ay;Ay];
w1=randn(1,50)*sqrt(0.05);
w2=randn(1,50)*sqrt(0.05);
w=[w1;w1;w2;w2];
x(:,1)=[0,1,10,0];
z(:,1)=[0,1,10,0];
for k=2:50;
x(:,k)=a*x(:,k-1)+A; % 真值
z(:,k)=x(:,k)+w(:,k-1); %测量值
end
%plot(x(1,:),x(3,:),z(1,:),z(3,:),'r')%
Q=0.05*eye(4);
H=eye(4); %观测方程Z=HX+R
R=0.05*eye(4);
I=eye(4);
Xt_1(:,1)=[0,1,10,0]; % X(0)
Pt_1=0.05*eye(4); %P(0)
for N=1:50;
Pt_=a*Pt_1*a'+b*Q*b'; %Pt_为P(n|n-1) Pt_1为P(n-1|n-1)
Kg=Pt_*H'*(H*Pt_*H'+R)^(-1);
Pt=(I-Kg*H)*Pt_; %Pt为P(n|n)
Xt_=a*Xt_1; %预测
Xt=Xt_+Kg*(z(:,k)-H*Xt_); %最优估计
Xyc(:,N)=Xt_; %输出预测
Xxz(:,N)=Xt; %输出最优估计
Pt_1=Pt; %更新P(n|n)
Xt_1=Xt; %更新X(n|n)
end
plot(Xxz(1,:),Xxz(3,:),'r',x(1,:),x(3,:),Xyc(1,:),Xyc(3,:),'g');
上面的程序出来的结果根本不是抛物线而是比较直的线。。。。不知道为啥啊。。。。 求大神解救啊
|
|