移动机器人

目录：

实现过程

假设这里有个机器人，我们要跟踪预测的机器人的量有两个：

p表示位置

v表示速度。

令X=[p,v]

我们有两个信息可以用：机器人轮子转动的情况和GPS传感器。也就是说，通过轮子转动情况，我们可以计算出机器人的位置和速度X0。通过GPS我们也可以得到一个位置和速度X1。

如果我们得到的X0和X1相等，我们就能够确定机器人的X

如果两个值不相等呢？机器人的位置应该是多少？

如何通过轮子转动的的情况和GPS数据来综合判定出机器人的位置和速度。

答案是通过卡尔曼滤波来判断。

位置和速度符合一个联合的二维的正态分布。

现在我们已知k-1的状态：X[k-1]=[[p[k-1],v[k-1]]

如何预测出k:X[k]=[[p[k],v[k]]?

其实我们只用之间的关系即可。

系如下：

所以，的关系如下（概率密度用均值向量，和协方差矩阵来表示）：

也就是说，k矩阵可以用（5）式来表示。（5）就是k。

这里扩展一下，如果我们给机器人加一个控制量：加速度a,那么k函数如下：

然后再给这个联合概率密度函数添加上高斯噪声：

通过所有传感器的状态与机器人状态之间的关系可以描述为：

但是通过状态预测得到传感器的读数，与真实状态之间可能会有误差。所以，上式与真实传感器读数是有差异的，

也就是真实的传感器读数的为zk,方差为Rk.

现在我们有两个联合概率密度函数：一个是由机器人状态计算得到传感器应该给出的状态的联合概率密度函数，这个是预测值，，预测传感器会给出的状态。

一个是传感器真实读数+噪声的联合概率密度函数，均值为zk,方差为Rk，这个是测量值。

一个测量值，一个预测值。

相乘的话，0乘任何值都为零，所以只有重合的部分有值

12，13中就是计算得到的最终的高斯分布。

注意到，两个高斯分布相乘后得到的高斯分布，均值和方差的变换都很有规律。

我们把公式整理一下：

这里，mu和sigma前要乘以Hk?因为mu和sigma表示的是实际的测量的均值和方差，或者称为观测得到的均值和方差。测量量与状态量之间的关系为Hk.

xk就是我们卡尔曼滤波预测得到的状态量。

我们回归一下卡尔曼滤波的工作过程：

一开始，有个状态量X0,以及它对应的方差P0，

我们利用固有的关系，比如速度，加速度与位置的关系F1,B1,u1，还有噪声，预测出一个状态量X1,以及P1，如下图，图中k=1.

然后再用X1,P1与测量值之间的固有关系H1，预测出测量值的概率密度函数，就是测量量的预测值。

实际的测量值符合(zk,Rk)分布。

测量量的预测值和与测量量的实际值的分布联合起来（具体就是相乘），我们就解析出了状态量最可能的值，如公式18，19所示。

k=1,我们就得到了X1的值。这个就是我们需要预测的状态值（速度和位置）

这里面已经量有：F1，B1，u1,Q1,H1(描述状态量与测量量之间的换算关系，已经知道）,(R1观测量的方差)

公式中的变量：x^k-1换成xk-1表示k-。x^k换成xk,表示k。
这样一改，就很清楚卡尔曼滤波的输入量是xk-1,输出量是xk了。输入k-1，输出k。
公式中的x^k,就表示在最终得到xk之前，单纯由xk-1,根据速度，加速度与位置之间的关系计算出的值，所以可以一个代表预测的符号^
这个x^k再经过真实测量值zk的纠正,就是卡尔曼真正地输出了xk了。
所以：卡尔曼的输入是xk-1,输出是xk,

从xk-1计算得出xk的过程中，

使用了测量值zk,

使用了速度，加速度与位置的固有关系Fk,Bk,uk,

使用了根据经验给出的噪声方差：Qk,Rk,

使用了状态量与观测量之间的固有关系Hk(这个关系也可能单纯是单位不一致，仅仅是一个尺度的关系，也可能是比较复杂的关系）

还有个问题

这样用状态的预测概率密度函数和测量值的概率密度函数共同表示最终的概率密度函数的方法，就是蒙特卡洛方法了，而这种方法又得到了形如：mu[

k]=mu[k-1]+K(zk-mu[k-1]),如公式(14),而这个公式与一般的滤波器的公式是一样的，就是一个线性变换，所以这种蒙特卡洛方法得到的最终概率密度，从而计算出状态量的方法，也就叫滤波了

如果用贝叶斯定理的角度来看卡尔曼滤波：

Fk描述的就是P(xt|ut,xt-1)，值之间的概率关系。

Hk描述的就是P(zt|xt),由状态值得到测量值的概率关系

关于贝叶斯滤波：

本文参考：

到目前为止，终于把原理过了一遍，可以把卡尔曼滤波与贝叶斯联系起来，也可以用高斯分布来具体估计一个。

Trackingofrotatingpoint.

Rotationspeedisconstant.

Bothstateandmeasurementsvectorsare1D(apointangle),

Measurementistherealpointangle+gaussiannoise.

Therealandtheestimatedpointsareconnectedwithyellowlinesegment,

therealandthemeasuredpointsareconnectedwithredlinesegment.

(ifKalmanfilterworkscorrectly,

theyellowsegmentshouldbeshorterthantheredone).

Pressinganykey(exceptESC)willresetthetrackingwithadifferentspeed.

PressingESCwillstoptheprogram.

对旋转运动的点point进行跟踪。

旋转速度是恒定的。

状态量和观察量是1d的，就是点的角度。

观察量是点的真正角度+高斯噪声。

真正的点与估计的点用黄色线连接。

真正的点与观察点用红色线连接。

如果卡尔曼滤波工作正常，那么黄色应该比红色短。

按任何键，可以用不同的速度重新跟踪。

按ESC，退出程序。

那就试试：pythonw

【下次更新】