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

卡尔曼滤波那点事---Android 9DOF算法卡尔曼滤波那点事---Android 9DOF算法11.前言12.Kalman Filter theory12.1.General12.2.非线性的卡尔曼滤波方程52.3.9DOF算法一：严格的非线性KalmanFilter72.4.Indirect Kalman Filter for 3D Attitude Estimation93.Scilab代码113.1.卡尔曼滤波修正gyro bias和计算方向余弦113.2.Kalman滤波计算线性加速度23前言卡尔曼滤波其实是用测量量来对系统状态参数进行修正的一种方式。对于刚接触的人而言，最需要的是一个实际使用的实例分析。本文以android代码中的9DOF算法为例，从卡尔曼的基本原理，到非线性卡尔曼滤波方程，最后到android实际使用的Indirect Kalman filter。让大家能够看明白卡尔曼滤波是如何在实际工程中使用的。整文分两部分。第一部分为理论。重点把结果罗列了一下。本文不是教材，所以推导过程都省略了。第二部分是代码。代码有些奇怪，用scilab写的，而不是matlab。scilab是开源的类matlab的工具，对业余爱好者足够。语法也基本类似。文中的公式都标明了出处。我尽可能避免公式写错。如果有怀疑，可以和后面的代码对照。最后，如果有问题需要探讨，可以联系：bigstone1998@。不过回复可能很不及时。Kalman Filter theoryGeneralKalman filter首先是离散线性系统的kalman filter。下面就有。接下去是连续线性系统的kalman filter。这个把系统离散化就可以。下面公式来自Wiki。大家从教材或网上都可以查到。物理意义：特别说明一下P。P是covariance of X。例如，如果X是2维矢量，那么P就是一个2*2矩阵。含义是协方差。启动Kalman算法的时候，需要初始值。初始值应该理解为。因为它们都是非修正值。以g=1的星球上的自由落体为例，来说明Kalman Filter的物理意义。这个问题可以描述为：我们大致知道在g=1的星球上，一个物体在100米左右，可能有初速度1以内的位置开始下落。每1秒钟我们有机会对这个物体进行一次观测，但是观测的精度也是在1米以内。看看如果用Kalman Filter方式来修正我们的对物体运动的预测。物理规律大家都知道：把这个方程离散化，假设以1s为步长：以位置和速度为状态参量X=[y,v]’，可以得到：从这个状态方程可得到F，B，u。然后看测量方程。测量量是位置。位置用状态参量表示为：从测量方程可以得到H。至于测量的误差量W(k)，根据假设都是1米。所以R=1。观测值为：状态参量可以认为没有噪声，所以Q=0。初始位置估计为95米，初始速度估计为1。初始的状态变量的估计偏差位置为10米，速度为1米/秒。所以初始值为：[100,97.9,94.4,92.7,87.3]。这样，scilab的代码为：F=[1,1;0,1];B=[0.5,1];u=-1;Q=0H=[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 pointX22(:,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// updatey(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);// predictX21(:,i)=F*X22(:,i)+B*u;P21(:,:,i)=F*P22(:,:,i)*F+Q;end说明：scilab的运行值和这个稍微有些不同。但基本一样。图中Estimate其实是修正后的状态变量X22里面的位置值。另外，如果设置不同的初始值和初始估计误差，结果会有些不同，但是趋势一样。特别是初始估计误差。如果设置不合理，比如设置为全0，那么Kalman滤波的结果就会收敛的慢一些。所以初值的正确估计和合理设置也很重要。注意，P和Q的初始化，必定有一个不为0。如果全为0，就没有必要Kalman去修正了。非线