卡尔曼滤波那点事---Android 9DOF算法

卡尔曼滤波那点事---Android 9DOF 算法

卡尔曼滤波那点事---Android 9DOF 算法.......................................................................................1

1. 前言.................................................................................................................................................1

2. Kalman Filter theory.......................................................................................................................1

2.1. General.................................................................................................................................1

2.2. 非线性的卡尔曼滤波方程..................................................................................................5

2.3. 9DOF 算法一：严格的非线性 KalmanFilter....................................................................7

2.4. Indirect Kalman Filter for 3D Attitude Estimation............................................................10

的基本原理，到非线性卡尔曼滤波方程，最后到 android 实际使用的 Indirect Kalman filter。

让大家能够看明白卡尔曼滤波是如何在实际工程中使用的。

整文分两部分。第一部分为理论。重点把结果罗列了一下。本文不是教材，所以推导过程

都省略了。第二部分是代码。代码有些奇怪，用 scilab 写的，而不是 matlab。scilab 是开源

的类 matlab 的工具，对业余爱好者足够。语法也基本类似。

文中的公式都标明了出处。我尽可能避免公式写错。如果有怀疑，可以和后面的代码对照。

最后，如果有问题需要探讨，可以联系：bigstone1998@sina.com。不过回复可能很不及时。

2. Kalman Filter theory

2.1.General

Kalman filter 首先是离散线性系统的 kalman filter。下面就有。

非修正值。

以 g=1 的星球上的自由落体为例，来说明 Kalman Filter 的物理意义。这个问题可以描述为：

我们大致知道在 g=1 的星球上，一个物体在 100 米左右，可能有初速度 1 以内的位置开始

下落。每 1 秒钟我们有机会对这个物体进行一次观测，但是观测的精度也是在 1 米以内。

看看如果用 Kalman Filter 方式来修正我们的对物体运动的预测。

物理规律大家都知道：

把这个方程离散化，假设以 1s 为步长：

{

以位置和速度为状态参量 X=[y,v]’，可以得到：

从这个状态方程可得到 F，B，u。

然后看测量方程。测量量是位置。位置用状态参量表示为：

这样，scilab 的代码为：

说明：scilab 的运行值和这个稍微有些不同。但基本一样。图中 Estimate 其实是修正后的状

一些。所以初值的正确估计和合理设置也很重要。注意，P 和 Q 的初始化，必定有一个不

为 0。如果全为 0，就没有必要 Kalman 去修正了。

2.2.非线性的卡尔曼滤波方程

但是不幸的是，我们常遇到的是非线性的连续系统，如我们的 attitude estimation，就是一