卡尔曼滤波那点事---Android9DOF算法.docx

卡尔曼滤波那点事Android 9DOF算法 TOC \o 1-3 \h \z \u 卡尔曼滤波那点事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 9 3. Scilab代码 11 3.1. 卡尔曼滤波修正gyro bias和计算方向余弦 11 3.2. Kalman滤波计算线性加速度 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 以g=1的星球上的自由落体为例，来说明Kalman Filter的物理意义。这个问题可以描述为： 我们大致知道在g=1的星球上，一个物体在100米左右，可能有初速度1以内的位置开始下落。每1秒钟我们有机会对这个物体进行一次观测，但是观测的精度也是在1米以内。看看如果用Kalman Filter方式来修正我们的对物体运动的预测。 物理规律大家都知道： y 把这个方程离散化，假设以1s为步长： y 以位置和速度为状态参量X=[y,v]’，可以得到： X 从这个状态方程可得到F，B，u。 然后看测量方程。测量量是位置。位置用状态参量表示为： Z 从测量方程可以得到H。至于测量的误差量W(k)，根据假设都是1米。所以R=1。观测值为： 状态参量可以认为没有噪声，所以Q=0。 初始位置估计为95米，初始速度估计为1。初始的状态变量的估计偏差位置为10米，速度为1米/秒。所以初始值为：[100,97.9,94.4,92.7,87.3]。 X 这样，scilab的代码为： F=[1,1;0,1]; B=[0.5,1]; u=-1; Q=0 H=[1,0]; Z=[100,100,97.9,94.4,92.7,87.3]; R=[0,1,1,1,1,1]; X21=[]; X22=[]; y=[]; S=[]; K=[]; P21=[]; P22=[]; I=[1,0;0,1]; ///////////////////////////////////// X21(:,1)=[95,1];// start point X22(:,1)=[95,0]; y(1)=0; S(1)=0; K(:,1)=[0,0]; P21(:,:,1)=[10,0;0,1]; P22(:,:,1)=[0,0;0,0]; fori=2:6 // update y(i)=Z(i)-H*X21(:,i-1); S(i)=H*P21(:,:,i-1)*H+R(i); K(:,i)=P21(:,:,i-1)*H/S(i); X22(:,i)=X21(:,i-1)+K(:,i)*y(i); P22(:,:,i)=(I-K(:,i)*H)*P21(:,:,i-1); // predict X21(:,i)=F*X22(:,i)+B*u; P21(:,:,i)=F*P22(:,:,i)*F+Q; end 说明：scilab的运行值和这个稍微有些不同。但基本一样。图中Estimate其实是修正后的状态变量X22里面的位置值。 另外，如果设置不同