Udacity机器人软件工程师课程笔记（三十二) - 卡尔曼滤波器 - 一维卡尔曼滤波器 - 多维卡尔曼滤波器 - 拓展卡尔曼滤波器（EKF）

一、概述

如wiki百科中介绍的那样，卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。

卡尔曼滤波在技术领域有许多的应用。常见的有飞机及太空船的导引、导航及控制。卡尔曼滤波也广为使用在时间序列的分析中，例如信号处理及计量经济学中。卡尔曼滤波也是机器人运动规划及控制的重要主题之一，有时也包括在轨迹最佳化。卡尔曼滤波也用在中轴神经系统运动控制的建模中。因为从给与运动命令到收到感觉神经的回授之间有时间差，使用卡尔曼滤波有助于建立符合实际的系统，估计运动系统的目前状态，并且更新命令。

二、一维高斯分布

高斯分布是概率分布，是一个连续函数。随机变量x取值介于$X_1$ 和$X_2$。
由以下函数的积分给出在$X_1$ 至 $X_2$中的x的概率:
$$p（X_1<X<X_2）={\int }_{X_1}^{X_2}{F}_X(x)dx$$
例如下图中，$x$位于8.7m和9m之间的概率为7％。

均值和方差

高斯的特征在于两个参数-平均值（$\mu$）和方差（$\sigma ²$）。平均值是最可能出现的值，位于函数的中心，并且方差与曲线的宽度有关。术语“单峰”表示分布中存在单个峰。

高斯分布通常缩写为$N（x：\mu ，\sigma ²）$，并将在以后以这种方式引用。

高斯分布的公式如下所示。 请注意，该公式包含二次函数的指数。 二次将x的值与μ进行比较，在x =μ的情况下，指数等于1（$e^0=1$）。

$p(x)=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$

所有选项的概率之和必须是1。因此，函数下面的面积总和总是1。

$\int p(x)dx=1$

接下来使用c++编写一维高斯函数，来计算一个值在给定平均值和方差的情况下出现的概率。

#include <iostream>
#include <math.h>

using namespace std;

double f(double mu, double sigma2, double x)
{
   
   
	double prob = 1.0 / sqrt(2.0 * 3.14159 * sigma2) * exp(-0.5 * pow((x - mu), 2.0) / sigma2);
	return prob;
}

int main()
{
   
   
	cout << f(10.0, 4.0, 8.0) << endl;
	return 0;
}

注：从此部分以后大部分程序将使用c++来编写

三、一维卡尔曼滤波器

变量命名约定

$x_t$:位置（state）
$z_t$:测量值(measurement)
$u_t$:控制作用(control action)

卡尔曼滤波循环

卡尔曼滤波的作用方式可以用以下的图表示

1.测量值更新

（1）平均值计算

假设由上述概率分布，有先验概率有$\mu =20，{\sigma }^2=9$的高斯分布，现有一个测量值，有$v=30，r^2=9$的高斯分布，则其平均值${\mu }^{\prime },{\sigma }^{2^{\prime }}$,可以以如下方式进行计算：
$${\mu }^{\prime }=\frac{r^2\mu +{\sigma }^{2}v}{r^2+{\sigma }^2}$$
先验的不确定性乘以测量的平均值，以赋予其更大的权重，同样，测量的不确定性乘以先验的平均值。
$${\sigma }^{2^{\prime }}=\frac{1}{\frac{1}{r^2}+\frac{1}{{\sigma }^2}}$$

（2）程序实现

接下来以先验概率$N(x:{\mu }_1=10,{\sigma }^2=8)$, 和测量值$N(x:{\mu }_1=13,{\sigma }^2=2)$为例，计算其平均值：

#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
   
   
	new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
	new_var = 1 / (1 / var1 + 1 / var2);
	return make_tuple(new_mean, new_var);
}

int main()
{
   
   

	tie(new_mean, new_var) = measurement_update(10, 8, 13, 2);
	printf("[%f, %f]", new_mean, new_var);
	return 0;
}

输出为

[12.400000, 1.600000]

2.位置预测

位置预测公式

后验均值：${\mu }^{\prime }={\mu }_1+{\mu }_2$

后验方差：${\sigma }^{2^{\prime }}={\sigma }_1^2+{\sigma }_2^2$

#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
   
   
    new_mean = mean1 + mean2;
    new_var =  var1 + var2;
    return make_tuple(new_mean, new_var);
}

int main()
{
   
   

    tie(new_mean, new_var) = state_prediction(10, 4, 12, 4);
    printf("[%f, %f]", new_mean, new_var);
    return 0;
}

3.一维卡尔曼滤波器

该代码将遍历可用的测量和运动 并对每个测量和运动应用测量校正或状态预测

#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
   
   
	new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
	new_var = 1 / (1 / var1 + 1 / var2);
	return make_tuple(new_mean, new_var);
}

tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
   
   
	new_mean = mean1 + mean2;
	new_var = var1 + var2;
	return make_tuple(new_mean, new_var);
}

int main()
{
   
   
	//Measurements and measurement variance
	double measurements[5] = {
   
    5, 6, 7, 9, 10 };
	double measurement_sig = 4;

	//Motions and motion variance
	double motion[5] = {
   
    1, 1, 2, 1, 1 };
	double motion_sig = 2;

	//Initial state
	double mu = 0;
	double sig = 1000;

	for (int i = 0; i < sizeof(measurements) / sizeof(measurements[0]); i++) {
   
   
		tie(mu, sig) = measurement_update(mu, sig, measurements[i], measurement_sig);
		printf("update:  [%f, %f]\n"