卡尔曼滤波算法与matlab实现

1、一个应用详细解释了卡尔曼滤波器及其算法的实现标签条：算法filtermatlabalgorithm优化塔斯克1033604875511人读书评论(25 )收藏通报分类：数据结构及其算法(四)为了使卡尔曼滤波器更容易理解，在此应用图像的记述方法进行说明。 并不像大部分的工具书那样数学公式和数学符号并列很多。 但是，他的五个公式是其核心内容。 配合现代电脑，其实卡尔曼的普拉姆很简单，只要你懂他的五个公式。在介绍他的五个公式之前，我们先根据下面的例子来探索一下一头地一头地。假设我们研究的对象是房间的温度。 根据你的经验判断，这个房间的温度是一定的，也就是说下一分钟的温度等于现在的温度(比如我们以一分

2、钟为单位)。 如果100%不相信你的经验，可能会上下错开好几次。 我们将这些个的偏差看作高斯合十礼噪声，即这些个的偏差与高斯分布(Gaussian Distribution )相符合，而与前后时间无关。 另外，我们在房间里放置了温度修正，但这个温度修正也不正确，测量值会偏离实际值。 我们也把这些个的偏差看作高斯白噪声。现在，关于某一部分，我们关于这个房间有两个温度值。 你的经验预测值(系统预测值)和温度修正值(测量值)。 将这两个值与各自的噪音组合起来估计房间的实际温度值。如果估计k的时间是实际的温度值。 首先，根据k-1时刻的温度值预测k时刻的温度。 因为相信温度是恒定的，所以假定k时刻的温

3、度预测值与k-1时刻相同，是23度，该值的高斯噪声的偏差是5度(5是，如果在k-1时刻估计的最适温度值的偏差是3，则对自各儿预测的不确定度是4度，他们将平方相加而恢复，然后温度因为估计k时刻的实际温度分别有23度和25度两个温度值。 实际的温度是多少？ 你相信自各儿还是温度校正？ 我们可以根据他们的协方差(covariance )来判断到底谁相信更多。 因为Kg2=52/(52 42)，所以Kg=0.78，k时刻的实际温度值可以估计为23 0.78*(25-23)=24.56度。 可以看出，因为温度校正的covariance相对小(相信温度校正)，所以估计的最适温度值偏向于温度校正的值。现在，

4、我们已经得到k时刻的最适温度值，下一步是进入k 1时刻，进行新的最佳估计。 到现在为止，好像没有出现什么自回归的东西。 但是，在进入k 1时刻之前，还计算k时刻的其最佳值(24.56度)的偏差。 修正算法如下： (1-Kg)*52)0.5=2.35。 其中，5是在上述的k时刻你预测的该23度温度值的偏差，得到的2.35是进入k 1时刻后在k时刻估计的最适温度值的偏差(与上述的3对应)。这样，卡尔曼滤波器递归地重复covariance，估计最佳温度值。 他很快就动了，只剩下上次的covariance了。 上面的Kg是卡尔曼男同志曼。 他可以随时改变自各儿的价格。 难道不奇怪吗现在回到正传，讨论真

5、正的工程系统上的卡尔曼。3 .卡尔曼滤波器算法(卡尔曼过滤器分配)本节介绍来自Dr Kalman的卡尔曼滤波器。 以下描述包括基本概念知识，例如概率、随机变量、高斯分布和状态空间，但对于卡尔曼滤波器的详细证明，在此不作描述。但也可以提供详细的证明，例如“概率”(Probability )、“随机变量”(Random Variable )、“高斯分布”(Gaussian Distribution )和“状态空间”首先引入离散控制过程的系统。 该系统可以用线性随机差分方程(linearstochasticdifferenceequation )来描述。x (k )=a x (k-1 )和b u (

6、k )中的一个加上系统的测量值Z(k)=H X(k) V(k )在上述式2中，X(k )是k时刻的系统状态，U(k )是针对k时刻的系统的控制量。 a和b是系统残奥仪表，在多模型系统的情况下是矩阵。 Z(k )是k时刻的测量值，h是测量系统的残奥仪表，对于多重测量系统，h是矩阵。 W(k )和V(k )分别表示进程和测量的噪声。 他们假设为高斯合十礼噪声，他们的covariance分别为q，r (这里假设他们不会随着系统状态的改变而改变)。为了满足上述条件(线性随机微分系统、过程和测量以及高斯合十礼噪声)，卡尔曼滤波器是最佳的信息处理器。 然后，将他们和他们的covariances组合起来估计

7、系统的最佳功率(如上一节中的温度示例所示)。首先，利用系统的进程模型，来预测下一状态的系统。 假设当前的系统状态为k，则可以根据系统模型的化学基，根据系统的前一个状态来预测出现状态x(k|k-1)=ax(k-1|k-1)bu(k)(1)在式(1)中，X(k|k-1 )是利用前一状态进行了预测的结果，X(k-1|k-1 )是前一状态最好的结果，U(k )是当前状态的控制量，如果没有控制量到目前为止我们的系统结果被更新了，但是与X(k|k-1 )对应的covariance (协方差)还没有被更新。 用p表示covariance。p (k|k-1 )=AP (k-1|k-1 ) aq(2)在式(2)

8、中，P(k|k-1 )是与X(k|k-1 )对应的差值，P(k-1|k-1 )是X(k|k-1 )的式1、2是卡尔曼滤波器的五个式子中的前两个、即系统的预测。得到当前状态的预测结果，收集当前状态的测量值。 将预测值和测量值相加，可得到当前状态(k )的最佳估计值X(k|k )。x (k|k )=x (k|k-1 )千克(k ) (z (k )-hx (k|k-1 ) )(3)其中，Kg是卡尔曼男同志：kg (k )=p (k-1 ) h/(HP (k-1 ) hr )(4)在此之前，我们在k状态下得到了最优估计值X(k|k )。 但是，为了使其他卡尔曼滤波器在系统进程结束之前持续运行，需要在k

9、状态下更新X(k|k )的covariance。p(k|k)=(i-kg(k)h(k|k-1)(5)在这里，I是1的矩阵，在单模型单测量中，I=1。 在系统进入k 1状态时，P(k|k )是式(2)的P(k-1|k-1 )。 由此，算法能够继续进行自回归的运算。基本上，卡尔曼滤波器的原理表明式1、2、3、4和5是其他五个基本式。 根据这些个5个式，能够容易地实现修正计算机的程序计程仪。接下来，将给出在Matlab计程仪程序中实际执行的示例。4 .简单的例子(简单示例)在此，结合第23节，举出非常简单的例子来说明卡尔曼滤波器的动作过程。 举个例子，进一步说明第2节的例子，分配方案模拟结果。根据第

10、2节的说明，把房间看作系统，对其系统进行模型化。 当然，我们看到的模型不一定非常准确。 我们知道这个房间的温度和之前时间的温度一样，所以A=1。 因为没有控制量，所以U(k)=0。 因此，情况如下：x(k|k-1)=x(k-1|k-1)(6)公式(2)可以变更为：P(k|k-1)=P(k-1|k-1) Q (7)由于测定的值是温度补正，与温度直接对应，因此H=1。 公式3、4和5可以改为：x (k|k )=x (k|k-1 )千克(k ) (z (k )-x (k|k-1 ) )(8)Kg(k)=P(k|k-1)/(P(k|k-1) R) (9)P(k|k)=(1-Kg(k)P(k|k-1)

11、(10 )在此，将一系列的测量值作为输入进行模拟。 我们假定房间的实际温度为25度，模拟了200个测量值。 这些个的测量值的平均值为25度，但标准离差有几度的高斯合十礼杂讯(图中为蓝线)。为了使卡尔曼滤波器工作，需要告诉卡尔曼两个零时间节点的初始值，即X(0|0)和P(0|0)。 你不用太在意他们的价格。 因为随着卡尔曼的工作，x逐渐收敛。 但是，对于p，通常不要取0。 这是因为完全相信卡尔曼给你的X(0|0)是最适合系统的，算法可能不收敛。 选择X(0|0)=1度，P(0|0)=10。该系统的实际温度为25度，在图中用黑线指示。 图中的红线是卡尔曼滤波器输出的优化结果(这个结果被设置为算法Q

12、=1e-6、R=1e-1 )。清除器N=200；w(1)=0；w=randn(1，n )x(1)=0；a=1；for k=2:N；x(k)=a*x(k-1) w(k-1 )。结束V=randn(1，n )；问题q1=std(V )；Rvv=q1.2；问题q2=std(x )；Rxx=q2.2；问题3 :Rww=q3.2；c=0.2；Y=c*x V；p(1)=0；s(1)=0；for t=2:N；p1(t)=a.2*p(t-1) Rwwb(t)=c*p1(t)/(c.2*p1(t) Rvv )。s (t )=a * s (t-1 ) b (t ) * (y (t )-a * c * s (t-1

13、 ) )。p(t)=p1(t)-c*b(t)*p1(t )。结束t=1:N；出图(t，s，r，t，y，g，t，x，b ) :用matlab创建的kalman过滤器通过了测试- -以下是Matlab源计程仪程序，可以更有效地查看。清除器clc；N=300；CON=25； %房间的温度。 假设温度是恒定的百分之百，百分之百，百分之百，百分之百。x=零(1，n ) :y=20.5 * randn(1，n )计算机。 %加法过程噪声的状态输出x(1)=1；p=10；Q=cov(randn(1，n): %进程噪声协方差R=cov(randn(1，n): %观测噪声协方差for k=2 : Nx(k)=x

14、(k - 1): %预估k时刻状态变量的值p=p Q； %对应于估计的协方差kg=p/(p R )； % Kalman增益x (k )=x (k )公斤* (y (k )-x (k ) )。p=(1-千克) * p；结束百分之百，百分之百，百分之百，百分之百。过滤器_ wid=10；smooth_res=zeros(1，n ) :for I=过滤器_ wid1: n时间=0；j=I -过滤器_ wid : I-1时间=时间y (j )；结束smooth _ RES (I )=时间/过滤器_ wid。结束% figure(1)；% hist(y )；t=1:N；figure(1)；exp值=ze

15、ros (1，n )；for i=1: Nexp值(I )=con；结束打印值、r、t、x、g、t、y、b、t、smooth_res、k )；legend (已执行、估计、测量、移动结果)；axis(0 N 20 30 )。标签(抽样时间)；ylabel (机器人)；标题(smoothfiltervskalmanfilter )；卡尔曼滤波器算法-核公式推导导论再现红旗第一篇：这是我的第一篇专栏文章，为我提供了一个可以分享知识的平台，这是我的阿里嘎多。 本主儿开始不定期更新文章，文章内容应集中在汽车动力控制、整车软件架构和软件支重轮等方面。 作为在校硕士，有许多理解不完全、可能错误，大家持不同意见欢迎讨论。 阿里嘎多！ -卡尔曼滤波器算法很牛逼。 因为有很多公式，所以有很多符号。 看上去牛很强啊。 乍一看不懂的东西都很牛本文导出了卡尔曼滤波器算法的核心公式，使其不被华丽的外表所震惊。 (随后拟写针对非线性情况的EKF和UKF，对卡尔曼滤波器算法作全面应用介绍。 有兴趣的东西可以关注专栏。 中所述)-Okay，进入正题。 这篇文章读者已经对卡