slam与kalman入门 卡尔曼滤波器matlab实现.ppt

卡尔曼滤波器matlab实现 卡尔曼滤波是线性无偏最小均方误差递推滤波器。是一种以状态变量的线性最小方差递推估算的方法。 卡尔曼滤波的5个基本公式 系统的状态方程： X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值： Z(k)=H X(k)+V(k) 我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态： 　　X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1) 用P表示协方差： 　　P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2) 　　A’表示A的转置矩阵，Q是系统过程噪声的协方差。式子1，2是卡尔曼滤波器对系统的预测。 得到现在状态k的最优化估算值X(k|k)： 　　X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) 其中Kg为卡尔曼增益(Kalman Gain)： 　　Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4) R是系统测量噪声的协方差，k状态下X(k|k)的协方差： 　　P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5) 　　其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。 　　 简单限定条件的matlab实现 clear N=500; w=0.1*randn(1,N);%过程噪声，返回一个1*n的随机项矩阵 v=10*randn(1,N);%测量噪声 x(1)=25; a=1; c=1; for k=2:N; x(k)=a*x(k-1)+w(k-1);end%线性随机微分方程 for k=1:N; Y(k)=c*x(k)+v(k);end%观测方程 p(1)=10; s(1)=1; for t=2:N; Rww=cov(w(1:t));求方差 Rvv=cov(v(1:t)); p1(t)=a.^2*p(t-1)+Rww;%2 b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%4求Kg s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));%3 p(t)=p1(t)-c*b(t)*p1(t);%5 end t=1:N; error=s-x;%估计值减去状态值 plot(t,s,‘-’,t,Y,‘:’,t,x,‘--’,t,error,‘*’);%实线卡尔曼，点线观测值，虚线状态值 legend(kalman estimate,observation,state value ); Slam机器人的建模 SLAM (simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。 建模主要参考 骆云祥《非线性滤波在移动机器人SLAM中的应用》 一.地图表示方法 1.栅格地图 将环境划分为方格，对每个方格以坐标表示，并赋予一定数值，表示该方格被物体占据的概率。易于创建和维护。 2.特征地图 用有关的几何特征(如点、线、面等)表示空间环境的特征。适用于定位。 3.拓扑地图 图中的节点对应环境中的特定地点或物体等特征，边表示不同节点之间的关系。 二.系统建模 Slam构架 1.坐标系模型 机器人位姿用一个三位状态向量表示： 2.里程计或控制命令模型 简单是说就是记录机器人运行一段时间内行驶距离和偏转角，或 线速度V，时间间隔ΔT，偏转角α。 3.运动模型 机器人运动模型的离散时间差分方程表示： 即控制命令。 采用基于控制命令运动模型表示机器人的运动方式，在控制输入 的作用下，机器人的运动模型就可以表示为： 为噪声项，描述轮子打滑等一些未知因素。L为两轴之间距离。 4.传感器观测模型 机器人所携带的测距传感器一般为激光雷达，其观测量Z是环境特征相对于机器人的极距离和方向角，在极坐标系中可表示为: 观测模型为： 为观测到的第i个特征的在全局坐标系的位置坐标， 为观测噪声。观测模型是描述传感器观测的环境特征与机器人当前位置的相互关系。 5.环境特征的增广模型 机器人在运行过程中，观测到一个新的环境特征时，就要把所观测到的新环境特征加入到系