python卡尔曼滤波融合_数据融合之卡尔曼滤波示例

哎，难受，知乎把我的完美格式打乱成这样子。

卡尔曼几乎是做数据融合都知道的一个算法了把。卡尔曼滤波能够实现从有噪声的传感器数据中获取我们的较为准确的信息。而且通过卡尔曼，我们甚至能够去推算一些不可直接测量的值。

1.高斯知识的简单回顾

在卡尔曼滤波器中，我们的任务就是估计出物体最佳的期望以及方差出来。接下来我们来些一个简单的高斯函数

def f(mu,sigma,x):

return 1/sqrt(2.*pi*sigma) * exp(-.5*(x - mu)**2/sig,a2)

均值代表你的最佳估计，方差代表你的不确定性

对于多元的高斯分布，如果高斯分布是斜的，表示两个维度的不确定性是相关的

高斯另外一个条件概率的性质：

这个可以看作是给定的信息下，能够推测出的概率是多少。

以上是一个两个高斯信号的条件分布，对于条件分布，如果对于变量B，B我们可以看成是传感器提供的信息，我们非常非常不确定的话，那么自然很大很大，这样的逆就很小很小，最后其实对均值和方差那一项就没有影响了，也就是说B没有提供任何信息量。

这个公式反应了两个高斯信号相乘的话，会得到一个方差更小的高斯分布，直观上的理解就是我们的信息更多了,那么我们的不确定性也就减少了，这实际上也是卡尔曼滤波的更新过程的简单版

现在让我们写一个更新的函数

def update(mean1,var1,mean2,var2):

new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)

new_var = 1 / (1 / var1 + 1 / var2)

return [new_mean new_var]

2. 一个移动机器人定位的例子

这个例子大家都应该见过，也就是一个不断测量和预测的过程实现定位的过程。

至于为什么运动会影起方差的减少，可以看下下面的图的过程。

以上是我们的运动更新，可以看到 由于运动的不确定性，它会导致运动之后的不确定性增加，也就是方差变大

def predict(mean1,var1,mean2,var2):

new_mean = mean1 + mean2

new_var = var1 + var2

ret