卡尔曼滤波那点事一一一Android 9DOF算法.docx

卡尔曼滤波那点事Android 9DOF算法  TOC \o 1-3 \h \z \u  HYPERLINK \l _Toc375336419 卡尔曼滤波那点事Android 9DOF算法  PAGEREF _Toc375336419 \h 1  HYPERLINK \l _Toc375336420 1. 前言  PAGEREF _Toc375336420 \h 1  HYPERLINK \l _Toc375336421 2. Kalman Filter theory  PAGEREF _Toc375336421 \h 1  HYPERLINK \l _Toc375336422 2.1. General  PAGEREF _Toc375336422 \h 1  HYPERLINK \l _Toc375336423 2.2. 非线性的卡尔曼滤波方程  PAGEREF _Toc375336423 \h 5  HYPERLINK \l _Toc375336424 2.3. 9DOF算法一：严格的非线性KalmanFilter  PAGEREF _Toc375336424 \h 7  HYPERLINK \l _Toc375336425 2.4. Indirect Kalman Filter for 3D Attitude Estimation  PAGEREF _Toc375336425 \h 9  HYPERLINK \l _Toc375336426 3. Scilab代码  PAGEREF _Toc375336426 \h 11  HYPERLINK \l _Toc375336427 3.1. 卡尔曼滤波修正gyro bias和计算方向余弦  PAGEREF _Toc375336427 \h 11  HYPERLINK \l _Toc375336428 3.2. Kalman滤波计算线性加速度  PAGEREF _Toc375336428 \h 23  前言 卡尔曼滤波其实是用测量量来对系统状态参数进行修正的一种方式。对于刚接触的人而言，最需要的是一个实际使用的实例分析。本文以android代码中的9DOF算法为例，从卡尔曼的基本原理，到非线性卡尔曼滤波方程，最后到android实际使用的Indirect Kalman filter。让大家能够看明白卡尔曼滤波是如何在实际工程中使用的。 整文分两部分。第一部分为理论。重点把结果罗列了一下。本文不是教材，所以推导过程都省略了。第二部分是代码。代码有些奇怪，用scilab写的，而不是matlab。scilab是开源的类matlab的工具，对业余爱好者足够。语法也基本类似。 文中的公式都标明了出处。我尽可能避免公式写错。如果有怀疑，可以和后面的代码对照。 最后，如果有问题需要探讨，可以联系：bigstone1998@。不过回复可能很不及时。 Kalman Filter theory General Kalman filter首先是离散线性系统的kalman filter。下面就有。 接下去是连续线性系统的kalman filter。这个把系统离散化就可以。 下面公式来自Wiki。大家从教材或网上都可以查到。 物理意义： 特别说明一下P。P是covariance of X。例如，如果X是2维矢量，那么P就是一个2*2矩阵。含义是协方差。 启动Kalman算法的时候，需要初始值。初始值应该理解为Xk|k-1, Pk|k-1。因为它们都是非修正值。 以g=1的星球上的自由落体为例，来说明Kalman Filter的物理意义。这个问题可以描述为： 我们大致知道在g=1的星球上，一个物体在100米左右，可能有初速度1以内的位置开始下落。每1秒钟我们有机会对这个物体进行一次观测，但是观测的精度也是在1米以内。看看如果用Kalman Filter方式来修正我们的对物体运动的预测。 物理规律大家都知道： y=-g=y(t)=yt0-g(t-t0)yt=yt0+yt0t-t0-g2(t-t0)2 把这个方程离散化，假设以1s为步长： yk+1=yk+yk-g/2yk+1=yk-g 以位置和速度为状态参量X=[y,v]’，可以得到： Xk+1=1101Xk+0.51-g 从这个状态方程可得到F，B，u。 然后看测量方程。测量量是位置。位置用状态参量表示为： Zk=10Xk+W(k) 从测量方程可以得到H。至于测量的误差量W(k)，根据假设都是1米。所以R=1。观测值为： 状态参量可以认为没有噪声，所以Q=0。 初始位置估计为95米，初始速度估计为1。初始的状态变量的估计偏差位置为1