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

return [new_mean,new_var]

现在我们我们来实现一个一维的卡尔曼滤波器，运用我们刚才的 预测函数(运动模型) 以及 更新函数(测量模型)，假定初始的位置为0，方差10000，

measurements = [5., 6., 7., 9., 10.]

motion = [1., 1., 2., 1., 1.]

measurement_sig = 4.

motion_sig = 2.

mu = 0. #init pos

sig = 10000. #init variance

for i in range(len(measurements)):

[mu, sig] = update(mu, sig, measurements[i], measurement_sig)

print('update:',[mu, sig])

[mu, sig] = predict(mu, sig, motion[i], motion_sig)

print('predict:',[mu, sig])

如果初始值是5的话，那么结果会是非常好，但是这里我们将初始值设置为0，且不确定性特别高。

运行程序 你会发现你的第一次位置估计结果差不多是5，原因在于你的初始不确定性太高了，根据我们刚才的公式就可以测量出来了。 第一次测量主导了结果 ，而且你的不确定性也降了很多，可以根据update那个数学公式推测出原因。

update: [4.999000199960007, 1.9996000799840032]

predict: [5.999000199960007, 3.9996000799840035]

update: [5.999500074988752, 1.9999000149977504]

predict: [6.999500074988752, 3.9999000149977504]

update: [6.999750034370274, 1.9999750034370276]

predict: [8.999750034370274, 3.9999750034370276]

update: [8.999875016794618, 1.9999937508397312]

predict: [9.999875016794618, 3.999993750839731]

update: [9.999937508348493, 1.9999984377087126]

predict: [10.999937508348493, 3.9999984377087126]

简单描述下上面这个例子

假设一开始处于0位置，但是不确定性非常高。

接下来进行第一次测量，可以看到变为4.999，方差下降为1.996.

接下来motion 了 1 步，机器人的位置变为5.999，方差增加为3.9996.

这个过程不断循环下去。

目前为止，我们已经实现了一个1D的卡尔曼滤波，总结起来就是

测量会减少系统的不确定性

预测会增加系统的不确定性(也就是运动的时候)

现在考虑2D的情况，想象我们有一个雷达，能够测出物体的位置，卡尔曼可以允许我们预测物体的速度，然后根据速度对未来做出更好的预测。 注意传感器本身只能看到位置，但是看不到速度，速度是从看到的多个位置推断出来的，在跟踪的应用当中，即使它没有直接测量，也能够得出。

This set of equation has been able to propagate constrains from subsequent measurements back to this unobservable variable , velocity ,so we can observe the vehicle also . This is really key to understand Kalmen filter .

But because those 2 thing interact , subsequent observations of the observable variable give us information about these hidden variables ,so we can also estimate what these the hidden variables are .

只要这些量是相关的，其实我们就可以推断出来啦。毕竟建立了微分方程之后，肯定就可以了呀

这个可以看下Sebatian 的视频

3.运动模型

4.测量模型

5.测量模型

测量模型根据你传感器的不同也有很多变化，比如激光返回位移+角度，摄像头返回像素点，具体的模型其实都相当成熟了。

6.贝叶斯滤波框架

刚才讲的卡尔曼例子是属于贝叶斯框架里面的。

贝叶斯滤波器是递归状态滤波器的一个框架，是一大类方法的统称，但是它是有不同的实现方式

可以根据以下属性进行分类：运动模型与观测模型 是线性还是非线性

噪声是否是高斯分布

参数滤波器还是非参数滤波器

参数滤波器有卡尔曼，拓展卡尔曼以及粒子滤波

非参数滤波的话有粒子滤波器

当然现在用的很广泛的一种优化算法，图优化，就不属于滤波器的理论了。我们可以发现滤波器的这种方法是没办法纠正前面的状态的，因为它是一个不断随着时间不断推进的一个状态估计。但是图优化的话它能够不断调整整体的误差，能够实现全局优化的效果。

7.卡尔曼滤波

接下来我们先来讲解一个

卡尔曼滤波器是线性高斯情况下的最优估计器了。

重点来看下卡尔曼增益的理解

对于卡尔曼增益K ，实际上是对预测值以及测量值 这两者的权衡。我们通过两个极端的情况来看下这个参数的作用：假设我们有一个完美的传感器

假设我们有一个很垃圾的传感器，不能提供任何信息，这种情况相当于没传感器

假设我们有一个完美的传感器，那么我的 也就是我传感器的方差为0，说明测量的东西非常准确，没有一点儿偏差。上式就变为

实际上做的就是将系统状态的均值投影到测量空间中

实际上是把系统的方差投影到测量空间中

公式的简单推导

预测模型

观测模型

8.卡尔曼滤波的一个例子

接着来我们以行人惯导为例，介绍以下行人惯导中的卡尔曼滤波

行人惯导里面，我们只使用IMU，没有外加其他传感器，这样做的原因是想要减少行人定位的成本，行人惯导里面主要也是研究如何低成本地进行定位 。 这种把惯导固定再鞋子有一个优点，就是我们能够检测出零速时刻， 可以看下下图直观感受下，当左脚站直，右脚往前迈的时，实际上就是左脚零速的时候，通过这种方式来检测出的零速可以作为我们的观测量 。

P：为什么之前做行人惯导要进行零速检测呢？

实际上就是我们的观测量就是零速。然后与我们估算的速度做比较，算出误差。

P：为什么要加雷达能提高精度？

实际上观测量就是距离了，与我们估算的位移做比较，算出误差。

贴一个大概的代码，来自openshoe

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Zero-velocity update %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Check if a zero velocity update should be done. If so, do the

% following

%%对于预测的部分我们不需要看了，因为实际上我们的惯导就是我们每一步的预测了。

if zupt(k)==true;

% Calculate the Kalman filter gain

K=(P*H')/(H*P*H'+R);

% Calculate the prediction error. Since the detector hypothesis

% is that the platform has zero velocity, the prediction error is

% equal to zero minu the estimated velocity.

z=-x_h(4:6,k);

% Estimation of the perturbations in the estimated navigation

% states

dx=K*z;

% Correct the navigation state using the estimated perturbations.

% (Subfunction located further down in the file.)

[x_h(:,k) quat]=comp_internal_states(x_h(:,k),dx,quat); % Subfunction located further down in the file.

% Update the filter state covariance matrix P.

P=(Id-K*H)*P;

% Make sure the filter state covariance matrix is symmetric.

P=(P+P')/2;

% Store the diagonal of the state covariance matrix P.

cov(:,k)=diag(P);

end

9.卡尔曼滤波的使用

P：实际用卡尔曼滤波的时候是什么情况呢？

我们要确定好状态变量是什么？确定好我们的观测方程是什么？然后就可以套公式了。

P：另外一个值得注意的问题是：

确定过程噪声以及测量噪声是什么？这个会在一定在影响我们的估计值？

确定过程噪声是一个比较重要的过程，可以看下我的UKF最后的讲解。

推荐阅读：

1.机器人学中的状态估计

2.概率机器人

3.Udacity上面有AI for robotics

If you'd like to watch another set of videos on Kalman filters, you might check out:

The Kalman Filter from iLectureOnline

给刚学习卡尔曼的建议：

第一步：先去Udacity看下视频先

第二步：去bilibili看下另外的卡尔曼例子

第三步：推导以下理论公式

第四步：找个数据集在Matlab上做下简单的仿真

在这个过程中，反复琢磨卡尔曼里面的协方差，卡尔曼的线性高斯是什么意思，卡尔曼增益代表什么。

恭喜你已经掌握卡尔曼滤波。