机器人应用

53 – 贝叶斯滤波到粒子滤波

/0 评论/在： , /通过：

1、 状态估计

估计，就是从带随机噪声干扰的观测信号中提取有用信息。
估计分状态估计和参数估计两类：
状态估计是随时间变化的随机过程，
参数估计是不随时间变化或变化极慢的随机变量。

状态估计，就是根据受干扰的观测数据来估计关于随机变量或系统的特性。
像贝叶斯状态估计，及其派生的卡尔曼系列的滤波、粒子滤波都可以解决状态估计问题：
如果系统具有线性和高斯性，则考虑卡尔曼滤波；
如果处理非线性滤波问题，有扩展卡尔曼滤波；
如果非线性、非高斯，则考虑粒子滤波。

2、 最优估计

应用中，总希望估计出来的参数或状态愈接近真实值愈好。这个好得程度如何度量，就是最优估计问题。
最优估计是针对某一估计准则而言的，选择合理的估计准则很重要。
估计准则可分为非贝叶斯估计准则和贝叶斯估计准则：
前者非贝叶斯估计准则常见有最小二乘准则和极大似然准则；
后者贝叶斯估计准则常见有极大后验准则和最小方差准则。

2.1、非贝叶斯估计准则
非贝叶斯估计准则常见的有最小二乘准则和极大似然准则

2.1.1、最小二乘估计
由高斯提出：
对于非时变的n维随机向量X，对其进行k次线性观测得m维观测向量Y，有Yi=Hi*Xi+Vi，则可能由Y来估计X。对于一个X的估计X~，如果（Y-HX`）^t*（Y-
HX`）达到极小，则称X`为X的最小二乘估计或加权最小二乘估计。
注意点：必须保存和处理所有观测数据Y。
2.1.2、极大似然准则
由费希尔首次使用：
对于n维待估计量X，Y为X的k次独立同分布的观测数据，则定义似然函数为L（X）= ∏[ p(Yi|X) ]，如果样本集上的一个函数X`=f（Y1…Yk）使得L（X`）最大，则X`为X的极大似然估计。

2.2、贝叶斯估计准则
设X为待估计量，X的先验分布为 p(X)，k个观测值Y，Y的条件概率密度分布为p(yi|X)，则由贝叶斯公式知后验概率p(X|y1…yk)=p(y1…k|X)p(X)/p(1…k)，对此后验概率估计X`用于估计X的散失，定义为损失函数为L（x，x`），则此损失函数的期望E（L（x，x`））即贝叶斯后验风险，对于使得此风险最小的估计为贝叶斯估计。
根据损失函数的形式，有不同的贝叶斯估计结果，常见的有极大后验准则和最小方差准则。

2.2.1、最大后验估计
如果设损失函数为0-1风险，即损失函数L(x`,x)使得x`=x为1，x！=x则为0，则显然使贝叶斯后验风险达到最小即要求后验概率密度p(X|Y1…yk)最大，这种估计称为最大后验估计。
如果对X没有先验知识，最大后验估计就退化为极大似然估计，即极大似然估计是一种特殊的最大后验估计。
一般情况下由于考虑了X的先验分布），一般的最大后验估计优于极大似然估计。
2.2.2、最小方差准则
如果损失函数为二阶欧式距离，即L(x`,x)=(x`-x)^t*(x`-x)，则此准则为最小方差估计。

3、蒙特卡洛采样

采样的目的，是评估一个函数在某个分布之上的期望值。

例如抛硬币：
期望的结果，是符合一个出正面的概率为p反面概率为1-p的伯努利分布。
现实中，可以通过不断地进行抛硬币进行采样动作，从而评估这个概率p。
这个概率p，就是用出正面次数除以总次数的频率近似。

这个方法叫做蒙特卡洛。

4、蒙特卡洛积分

蒙特卡洛法就是：
对于一个分布来说，如果想要知道这个分布的期望，可以通过撒粒子的样本方式。

例如：
要求一个稀奇古怪的平面形状的面积。如果没有解析的表达方法，那就用蒙特卡洛法，就是在一个包裹了这个形状的更大图形例如外接圆或者外接正方形内随机的撒点，然后统计落进图形里的个数，当撒的点够多时，待求小面积/已知大面积=(落在图形内的点的个数/撒出去的点的总个数)，即可求出待求小面积=(落在图形内的点的个数/撒出去的点的总个数)*已知大面积。

再例如：
要求一个稀奇古怪的函数f(x)在ab区间的积分。如果没有解析办法或者解析方法很难，蒙特卡洛法就是在积分区间随机撒点，然后计算每个点xi的f(xi)值————这个是不费力的，当点足够多的时候， 点个数/(b-a)=∑[f(xi)]/∫[f(x)dx]，即ab区间的积分∫[f(x)dx]值为 (b-a)/点的个数*∑[f(xi)]。

推广一般：
要求随机变量x的期望 E(x)=∫[xp(x)]dx；
或者求某随机变量x的函数f(x)的期望 E(f(x))=∫[f(x)p(x)]dx；
而通常这个随机变量x的已知的分布函数p(x)形式非常复杂的，通常很难进行几分。
此时可以使用经典的蒙特卡洛方法：
比照这个分布，采样某些样本，取其平均，近似所求积分。
即用∑[ f(xi) ] —> ∫[f(x)p(x)]dx
使用这种蒙特卡洛方法计算的积分，就称为蒙特卡洛积分。

4、重要性采样

以上蒙特卡洛积分很好很优秀，可是存在一个关于采样的问题，怎么才能按照p(x)这个分布生成样本？

如果概率分布p(x)具有标准固定的形式，比如高斯分布，那是可以直接从p(x)中抽取粒子的。
但是通常p(x) 的分布未知，或者已知但是十分复杂。此时可以生成N个随机数ti…n，然后根据随机变量X的分布函数f(x)计算得到累计概率分布F(x)，通过每个F(xi)=ti就可以求得n个采样的样本点xi，i=1…n。 但这个方法，从随机变量x的pdf计算cdf这本身就是个几分问题。

所以此时需要其他的采样策略进行采样，例如重要性采样BIS：

如果由于变量X的分布函数f(x)不具有解析性、或者可计算性较差，那么可以引入一个已知的、形态较好的分布函数q(x)，这样就很容易求随机变量x或其函数f(x)在q(x)上的期望了。

变量x的期望：
E(x)
= ∫ [xp(x)dx] <---同乘q(x) = ∫ [x * p(x) * q(x)/q(x) dx] <---移位 = ∫ [p(x)/q(x) * x*q(x) dx] 函数f(x)的期望： E(f(x)) = ∫ [f(x)p(x)dx] <---同乘q(x) = ∫ [f(x) * p(x)q(x)/q(x) dx] <---移位 = ∫ [p(x)/q(x) * f(x)*q(x) dx] 可见，求p(x)分布下f(x)的期望，变成了，求q(x)分布下p(x)/q(x)*f(x)的期望。

重要性采样的关键，是把不好求的p(x)分布下f(x)的期望，经过重要性采样，变成了在另一个分布q(x)下相对好求的另外形式的期望。
就是：
由q(x)得累计概率分布Q(x)，然后用上面提到的采样方法通过Q(x)获得样本点xi…n，而p(xi)/q(xi)*f(xi)容易计算，q(xi)也容易计算，
那么∑[ p(xi)/q(xi)*f(xi) * q(xi)]， 就可以近似E(f(x))。

5、样本权重

重要性采样里面分布函数q(x)的选择，是个重要的问题。
如何能够尽量接近随机变量X的分布函数p(x)，需要根据具体的情况斟酌选择。
像前面图，从p(x)采样的粒子点对应的f(x) 数值都较小，那么在采样数量N有限的情况下很可能会导致无法获得f(x) 的值较大的那些样本，这样评估出来的期望值就导致偏大。

因此，可以：
通过在更大范围内按照均匀分布随机采样，然后从均匀分布随机生成的样本按照一种比例于p(xi) 的权重修正。

或者：
用某种距离/相似度来构造度量函数，然后再把这些距离通过某种方式变换成概率分布。
例如视频跟踪，在某给定区域内寻找特定的已知的人像模板，则粒子的状态是位置(x, y)和大小(w,h)，那么计算每个粒子权重的方法是：
对每个粒子，求其直方图的向量，然后和模板的向量求余弦距离t；
通过通过常用似然的指数族函数W(t)=K*exp^(-alpha*t)构造权重函数，距离远值小距离近值大，其中K和alpha保证累计和为1；
这样对于每个样本点就都有了一个权重值W(t)；
对此权重值归一化处理，使所有样本粒子的权重值之和为1，则权重值处理成概率值WB(t)；
这个WB(t)就是需要的q(x)。

或者：
像粒子滤波pf使用的是特征。
在机器人定位中，绿点机器人被随机放到已知地图的某位置，蓝线是其激光测量线，求自身在地图位置。
撒N个红点代表机器人对自己位置的猜想，即蒙特卡洛的样本粒子；这粒子的分布是均匀分布，也就是∫ [p(x)/q(x) * f(x)*q(x) dx]
里面的q(x)；那么如果简单把所有粒子进行平均，则机器人定位要永远处于地图中心点。不过有权重值p(x)/q(x)存在，可以修正不同的红点粒子。但是，这个权重比值里面，只有q(x)明了，而p(x)是关于机器人在地图中的位置的概率、这个作为定位问题的关键企却是不知道的。
粒子滤波器pf对于这里的不知道这个问题，不像上面那样，先选择某种度量，例如欧氏距离，然后构造似然，例如指数族函数=K*exp^(-alpha*t)；

它采用的是特征作为权重值来修正均匀分布的q(x)的：
地图已知、激光探测的环境已知，所以可以把p(x)/q(x)理解为：当前激光所探测到的环境的特征、猜测的地图红点附近的环境的特征、这两者的相似程度，这样p(x)/q(x)的计算转为特征相似度的计算。

为了更好的性能，自然希望p(x)/q(x)尽量接近1，也就是使蒙特卡罗预设的q(x)要尽可能的接近原始随即变量的概率密度p(x)，
这是重采样的工作意义。

6、 重采样

45-卡尔曼滤波用于角度预测

/0 评论/在： , /通过：

网页不支持mathtype的公式，直接贴图。

 

 

C代码实现：

44-卡尔曼滤波器基本原理

/0 评论/在： , /通过：

1、 马尔科夫链
2、 状态机
3、 概率加权
4、 示例
5、 工程模型
6、 工程模型实例
7、 Matlab实现实例

卡尔曼滤波，和傅立叶变换、泰勒级数一样，是用卡尔曼的名字命名的。
它的基础是Wiener的维纳滤波理论，但维纳滤波需要用到无限个过去的数据，并不适用于实时处理。
而卡尔曼把状态空间模型引入滤波理论，从而克服这一缺点。

1、 马尔科夫链

如果当前状态St仅仅与上一个状态St-1有关，而与上上个状态St-2无关，更与上上上个状态St-3没有关系…，那么这些状态就是马尔科夫链。
马尔科夫过程已经非常成熟的应用于语音识别、输入预测等等。比如在搜索框里输入清华，则可以自动填补出后面大学两个字，等等。

马尔科夫过程适合使用状态及描述。

2、 状态机

卡尔曼的空间模型其实就是状态机。

例如，LabVIEW里面最重要的一个design pattern就是state machine状态机：
当前状态转换，依赖于上一状态。

再例如单片机，其串口通讯协议一般都用状态机模型来组包：
当前接收的hex字符是不是有用数据，这要考虑上一个状态。当前字符需要根据上一状态，才能决定是有用数据、用于组建命令字，还是无用数据、可以直接丢弃：
命令字组包

3、 概率加权

卡尔曼滤波器是一个最优化的自回归算法，目标是最小均方误差，基本思想是采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新状态变量的估计值。
这有些智能算法的味道，比如遗传算法，不同之处在于卡尔曼没有用到随机数。

这个滤波算法在近几十年来已经应用到机器人导航、传感器数据融合等场景，最近年来也被应用于计算机图像处理，例如人脸识别、图像分割等。

总结：
卡尔曼主要是：一状态机、二概率加权、三迭代。

4、 示例

验证卡尔曼滤波，最好是用ds18b20温度传感器，搭上一块单片机开发板，尝试预测一下房间的温度试试，预测结果准确与否，立刻可以检验。
用来校正小车或者飞行器的姿态作示例，反而一时不容易看到直接效果。

假设研究对象为某房间的温度。
一、根据已有经验房间温度变化是缓慢的、甚至可认为恒定，也就是说，下一时刻温度等于现在时刻的温度。但是这个经验并不是百分百可靠，还可能有上下几度的偏差，这些偏差可以看作高斯白噪声，即偏差跟前后时间没有关系、而且高斯分布，此时就有了预测温度和预测偏差。

二、另外房间里放置有一支温度计（观测器），但是温度计也不十分准确，测量值与实际值也有偏差，偏差也呈高斯白噪声，此时就又有了观测温度和观测偏差。

三、那么在任意一时刻都存在两个温度值和两个偏差，一是根据经验的预测值和预测偏差，二是观测设备的观测值和观测偏差，而卡尔曼根据这两对值，就可以估算房间的最优温度和最优偏差，这又有了第三组数据。

四、注意，涉及的温度：预测温度、观测温度、最优（估算）温度。

下面以k时刻的估算为例。

已知：上一状态k-1时刻已经有估算出来的最优温度为23度最优偏差为3度，预测温度NA度预测偏差4度，当前状态k时刻的观测温度25度观测偏差4度，那么：

一、使用上一状态k-1时刻的数据。根据过去k-1时刻的最优温度23度，直接可得现在k时刻的预测温度等于23度，由k-1时刻最优温度23度的最优偏差是3度和k-1时刻预测温度NA的预测偏差是4度，间接可得当前状态k时刻的预测偏差为sqrt（3^2+4^2）=5度，这样当前状态k时刻的预测温度23度和预测偏差5度均已获得。

二、使用当前状态k时刻的数据。根据温度计得到当前k时刻的观测温度（例如25度），同时该观测温度的观测偏差是4度。这样当前状态k时刻的观测温度25度和观测偏差4度均已获得。

三、上两步已经获得当前状态k时刻的预测温度和观测温度两组数据，现在需要求取最关键的最优温度。对于由k-1时刻得来的预测值23度和根据k时刻温度计所得的观测值25度，应该相信预测多一点还是相信观测多一点，这需要用到协方差来判断，用的是卡尔曼增益Kg Kalman Gain，Kg大代表误差大的一方，它会反方向的增大误差小的那一方的可信度，即Kg的权重总是乘在较可信的那一方，你的误差越大给对方乘的权重越大。

首先构建卡尔曼增益Kg = sqrt[ 5^2/(5^2+4^2) ] = 0.78，通过 0.78*25 + (1-0.78)*23 = 24.56，就可以估算出当前状态k时刻的最优温度24.56度。

然后由sqrt[ (1-Kg) * 5^2 ]=2.35度，就可以估算出最优温度的最优误差是2.35度。这样，当前状态k时刻的最优温度24.56度和最优偏差2.35度均已获得。

四、可以看出，因为预测偏差比较大（5度）、观测设备温度计的covariance比较小（4度），所以还是比较相信温度计的观测数据，结果也表明估算的最优温度值是偏向温度计的观测值的。

五、重新列一遍，回忆一下由k-1时刻到k时刻的情况：

上一状态k-1时刻的最优温度为23度最优偏差为3度，预测温度NA度预测偏差4度，当前状态k时刻的观测温度25度观测偏差4度，

那么此时由k时刻到k+1时刻（黑体是发生变化了的数据）：当前状态k时刻的最优温度为24.56度和最优偏差2.35度，预测温度23度和预测偏差5度。

如果下一状态K+1时刻有了观测温度？观测误差4度，就可估算出k+1时刻的数据。如果有了K+1时刻就可估算出K+2时刻…手工演算两行：

手工演算

其实就是迭代重新进入步骤一。

如果把分钟替换为天，把房间温度替换为城市温度，这其实就是一个简易的天气预报的模型。

5、 工程模型

工程系统上的卡尔曼滤波器算法，最核心的有5个计算公式。

首先，对于离散控制过程的系统，可用一个线性随机微分方程来描述：

X(k) = A*X(k-1) + B*U(k) + W(k)——关于系统的

Z(k) = H*X(k) + V(k)——关于观测的

X(t)是t时刻的系统状态，U(t)是t时刻对系统的控制量如果没有控制就是零，AB是过程参数，W(t)为过程噪声也就是预测偏差。Z(t)是t时刻的测量值，H是测量系统参数，V(t)为测量噪声也就是观测偏差。W(t)和V(t)两个噪声假设为高斯白噪声，它们的协方差分别是Q、R。

一、根据上一状态k-1时刻计算当前状态k时刻的预测值：

X(k|k-1) = A*X(k-1|k-1) + B*U(k) ——(1)

X(k-1|k-1)是上一状态k-1时刻已经估算出来的最优值，X(k|k-1)是利用k-1时刻预测k时刻的预测值，U(k)为当前状态k时刻的外加控制量、没有外部控制则为0，通过这个式子就更新了系统的状态。

二、还需要更新当前状态k时刻的预测值X(k|k-1)的协方差P：
P(k|k-1) = A*P(k-1|k-1) A’ + Q——(2)

P(k-1|k-1)是上一状态k-1时刻最优值X(k-1|k-1)的协方差，P(k|k-1)是当前状态k时刻预测值X(k|k-1)的协方差，Q是系统过程噪声的协方差即预测偏差。

三、然后由当前状态的观测值，结合预测值，得到当前状态k时刻的最优值X(k|k)：

X(k|k) = X(k|k-1) + Kg(k)*(Z(k) – H*X(k|k-1)) ——(3)或者表达为：

X(k|k) = [I – Kg(k)*H] * X(k|k-1) + Kg(k)*Z(k) ——(3)

Z(k)是当前状态的观测值，Kg位卡尔曼增益，此时已经估算出了当前状态的最优值X(k|k) 。这个式子比较关键。

四、更新卡尔曼增益Kg：

Kg(k) = P(k|k-1) *H’ / (H*P(k|k-1) H’ + R)——(4)

P(k|k-1)是当前状态k时刻预测值的协方差，R是观测过程噪声即观测偏差。

五、虽然已经得到了k状态下最优数据X(k|k)，但是为了迭代需要还要更新当前状态k时刻最优值X(k|k)的协方差：

P(k|k) =（I-Kg(k)*H） * P(k|k-1)——(5)

I为单位矩阵，或者对单变量就是标量1。

注意含义：P(k|k)是当前状态k时刻最优值的协方差，而P(k|k-1)是当前状态k时刻预测值得协方差。

六、不停递归即可：k、k+1、k+2…

6、 工程模型实例

前面上好佳实例里面，房间就是系统，对于该系统：只有一个变量，即温度；当前状态温度等于上一状态温度，所以系统参数A=1；没有外部控制，所以U(k)=0，系统参数B自然不需考虑。因此该系统的过程是：

X(k|k-1) = X(k-1|k-1)——式子1

上好佳实例里面，观测器就是温度计。因为温度计测量值就是温度不需变换，所以该系统的观测窗口H=1：

P(k|k-1) = P(k-1|k-1) + Q——式子2，Q过程噪声协方差

其他三个式子依次是：

X(k|k) = X(k|k-1) + Kg(k)*(Z(k) – X(k|k-1)) ——式子3或者表达为：

X(k|k) = [1 – Kg(k)] * X(k|k-1) + Kg(k)*Z(k) ——式子3

Kg(k) = P(k|k-1) / (P(k|k-1)  + R)——式子4，R观测噪声协方差

P(k|k) =（I-Kg(k)） * P(k|k-1)——式子5

7、 Matlab实现实例

m代码：
卡尔曼滤波器matlab实现

效果：
卡尔曼滤波器matlab实现效果

对于这种简单温度预测的算法，用C实现比较简单。

补充：
以上的协方差是广义的，很多时候就是方差，不是协。一个变量的方差可以理解成该变量和变量本身的协方差，并没错误。