1、卡尔曼滤波器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是
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实现,clea
3、rN=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;%2b(t)=c*p1(t)/(c.2*p1(t)+Rvv);%4求Kgs(t)=a*s(t-1)+b(t)*(Y(t)-
4、a*c*s(t-1);%3p(t)=p1(t)-c*b(t)*p1(t);%5endt=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可以描述为
5、:机器人在未知环境中从一个未知的位置开始移动，在移动过程中根据位置估计和传感器观测进行自身定位，同时建立环境地图。 滤波理论在SLAM中的应用。1.卡尔曼滤波 1）KF 2）EKF 泰勒级数一介项 3）UKF 采样策略逼近非线性分布，是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度。产生采样的非局部效应。2.粒子滤波PF（非线性、非高斯） 通过从后验概率中抽取的随机状态粒子来表达其分布 ，需要用大量的样本数量 。,建模主要参考 骆云祥非线性滤波在移动机器人SLAM中的应用,一.地图表示方法1.栅格地图 将环境划分为方格，对每个方格以坐标表示，并赋予一定数值，表示该
6、方格被物体占据的概率。易于创建和维护。2.特征地图 用有关的几何特征(如点、线、面等)表示空间环境的特征。适用于定位。3.拓扑地图 图中的节点对应环境中的特定地点或物体等特征，边表示不同节点之间的关系。,二.系统建模Slam构架,1.坐标系模型,机器人位姿用一个三位状态向量表示：2.里程计或控制命令模型 简单是说就是记录机器人运行一段时间内行驶距离和偏转角，或 线速度V，时间间隔T，偏转角。3.运动模型机器人运动模型的离散时间差分方程表示： 即控制命令。,采用基于控制命令运动模型表示机器人的运动方式，在控制输入 的作用下，机器人的运动模型就可以表示为： 为噪声项，描述轮子打滑等一些未知因素。L为两轴之间距离。,4.传感器观测模型 机器人所携带的测距传感器一般为激光雷达，其观测量Z是环境特征相对于机器人的极距离和方向角，在极坐标系中可表示为: 观测模型为： 为观测到的第i个特征的在全局坐标系的位置坐标， 为观测噪声。观测模型是描述传感器观测的环境特征与机器人当前位置的相互关系。,5.环境特征的增广模型机器人在运行过程中，观测到一个新的环境特征时，就要把所观测到的新环境特征加入到系统的状态向量中。这个新的环境特征在地图中的表示量是关于机器人当前位置和观测量的矢量函数:观测到的环境特征极坐标系中表示为 。若当前的机器人的位姿为 ，则新的环境环境特征在全局地图中表示 为:,谢谢,
