如何卡尔曼滤波器处理惯性测量得到的数据

1984pqp

简介
对我们来说, 卡尔曼滤波器看起来是空灵又瑰丽的一个概念。的确，它奇迹般的帮我们解决了一些较为棘手的问题。但是，它不是万能的，并不能帮我们解决一切我们面对的难题。
下面本文将解释卡尔曼滤波器是如何工作的。我们将采用实例的办法来帮助我们理解，而不是长篇累牍的讲令人讨厌而且晦涩的理论。因为我们大多数人只是在MAV/UAV（即微型飞行器/无人机）应用中才用到它，所以我会尽力使其看起来更具体和形象，而不是令人抽象的讲解。
假设大家都知道如何从加速度计和陀螺仪中得到相应的数据。一些基本的代数知识也会派上用场。:-)
基本运算
卡尔曼滤波是一个迭代器，需要两个数据。
首先，你将需要一些输入数据，这些输入数据可通过线性运算得到预期理想的输出。即：我们需对我们的问题抽象出一个线性模型。
第二，我们也需要其他的输入数据，这可能是预期的真值或非常接近真实值的近似值。每次迭代，卡尔曼滤波都会稍微改变改变线性模型的变量。所以我们线性模型的输出会和第二次的输入数据非常接近。

1984pqp

我们用一个简单的模型来解释卡尔曼滤波器的基本运算
显然, 我们的两个输入数据采用加速度计和陀螺仪输出的数据。我们模型所用的陀螺仪数据形式如下：

(原文件名:陀螺仪数据.png)
第一个公式表示线性模型的一般表示形式。我们需要填写A和B矩阵，并选择一个稳态x。
变量u 表示输入数据. 此处是陀螺仪的数据。 我们是如何合并这些数据的？ 我们只是添加了归一化测量。
alpha k = alpha k-1 + (_u_ k – bias)
我们需要包含两次测量(_dt_)之间的时间间隔dt，因为我们是在处理角速度。
alpha k = alpha k-1 + (_u_ k – bias) * dt
即:
alpha k = alpha k-1 – bias * dt + u k * dt

我们的偏置量是常量。对于陀螺仪来说，该偏置量就是偏置漂移量。好了， 我们了解了卡尔曼的精髓：该过滤器过比较加速度的输出结果（我们第二次输入的数据）来调整每次迭加的偏置量。
下面待续。。。睡觉了先。

litao8421

楼上这个是非典型Kalman filter在惯导的应用，期待下文。

哪天我写个典型的Kalman filter在惯导中应用。

aaa1982

期待下文

xj_1232005

顶，赞

zht9961020

翻译的不错

1984pqp

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2楼】 litao8421
楼上这个是非典型Kalman filter在惯导的应用，期待下文。
哪天我写个典型的Kalman filter在惯导中应用。  
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
期待典型的大作。。呵呵。别忘记告诉我一声啊。


结论
以下是一些利用到矩阵和统计的公式，现在我们不需要详细的知道这些公式如何来的。
u = measurement1    //读取最后得到的测量值
x = A · x + B · u  //更新模型的稳态x
y = measurement2  //读取第二次的测量值或真值.此处是加速度计的角度运算值
Inn = y – C · x  //第二次的值和用我们模型运算得到的预测值进行相减运算
s = C · P · C’ + Sz  //计算协方差
K = A · P · C’ · inv(_s_)  //计算卡尔曼增益
x = x + K · Inn  //纠正稳态的预测值
P = A · P · A’ – K · C · P · A’ + Sw  //计算预测误差的协方差

C矩阵是从状态矩阵的输出反推出来的。对我们来说，它是(1 0)’ :
alpha = C · x
Sz是测量过程中的噪声方差: Sz = E(zk zkT)
在我们的例子中，Sz是（加速度计数据中）抖动造成的噪声方差。
Sw是处理过程的噪声协方差矩阵（此处是一个 2×2的矩阵): Sw = E(x · xT)
故: Sw = E( [alpha bias]’ · [alpha bias] )
因为只有Sw矩阵对角线上的元素被用到，我们需要知道E(alpha2) and E(bias2),

陀螺仪的alpha中的噪声乘以dt2.故：E(alpha2) = E(u2)· dt2.
这些因子由我们所用传感器所决定。我们可通过实验计算出来。
自动驾驶仪等的卡尔曼滤波器的源代码中, 一般使用如下常量:
E(alpha2) = 0.001
E(bias2) = 0.003
Sz = 0.3 (radians = 17.2 degrees)

以上是翻译某国外高人的，限于水平，难免出错...(仅仅为了方便自己阅读，请不要拍砖，呵呵).
注：E(.)是统计函数，表示期望值。

lollipop

坐下听课，数学太烂了，这些东西一定要有人咬烂了我才能吃，=,=

1984pqp

PID是比例,积分,微分的缩写.
比例调节作用：是按比例反应系统的偏差,系统一旦出现了偏差,比例调节立即产生调节作用用以减少偏差。比例作用大,可以加快调节,减少误差,但是过大的比例,使系统的稳定性下降,甚至造成系统的不稳定。

积分调节作用：是使系统消除稳态误差,提高无差度。因为有误差,积分调节就进行,直至无差,积分调节停止,积分调节输出一常值。积分作用的强弱取决与积分时间常数Ti,Ti越小,积分作用就越强。反之Ti大则积分作用弱,加入积分调节可使系统稳定性下降,动态响应变慢。积分作用常与另两种调节规律结合,组成PI调节器或PID调节器。

微分调节作用：微分作用反映系统偏差信号的变化率,具有预见性,能预见偏差变化的趋势,因此能产生超前的控制作用,在偏差还没有形成之前,已被微分调节作用消除。因此,可以改善系统的动态性能。在微分时间选择合适情况下,可以减少超调,减少调节时间。微分作用对噪声干扰有放大作用,因此过强的加微分调节,对系统抗干扰不利。此外,微分反应的是变化率,而当输入没有变化时,微分作用输出为零。微分作用不能单独使用,需要与另外两种调节规律相结合,组成PD或PID控制器

huike

MARK

谢谢！

huike

楼主，标题是不是有点难读？

1984pqp

to huike:
翻译的不准确，不好意思，没注意。呵呵。

aaa1982

能指明一下原始出处么？感谢

lin1936

再学习

skystalker

原文在此ourdev_647343WM85SE.mht(文件大小:98K) (原文件名:MAV-blog  Kalman filtering of IMU data.mht)

autopass

mark

taogashi

如果dt是常数，A就是常数阵。
s = C · P · C’ + Sz  //计算协方差
K = A · P · C’ · inv(_s_)  //计算卡尔曼增益  
P = A · P · A’ – K · C · P · A’ + Sw  //计算预测误差的协方差
Sw、Sz有都是常数，可以预见给定一个初始P阵，K必然会趋于稳定。而且稳定值只跟Sz、Sw、P的初值选取有关，就是说，只要确定了Sz、Sw、P0，K的稳态值是可以算出来的。那么这样的“卡尔曼滤波”，还是卡尔曼滤波吗？凉粉吃进肚子里，还是凉粉吗？等高手解释啊………………

sunicecream

记号一下~

taogashi

刚补习了一下卡尔曼滤波原理，增益是可以离线计算的。关键是几个噪声方差怎么测量。

wshtmcz

学习下

highnose

赶快占座

谢谢楼主分享！

hitor

mark

3466756555

这么好的帖子 咋 么有下文了呢

breakor

mark！！！！！！！！！！！！！！

杨大侠

期待下文。

杨大侠

写得不错，期待下文。

chushibinsaobao

感谢大师分享，不过还是不是很懂