当前位置:首页 >> 信息与通信 >>

卡尔曼滤波那点事---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 ................................................... 9 3. Scilab 代码.............................................................................................................................. 11 3.1. 卡尔曼滤波修正 gyro bias 和计算方向余弦 ........................................................ 11 3.2. Kalman 滤波计算线性加速度 ............................................................................... 23

1. 前言
卡尔曼滤波其实是用测量量来对系统状态参数进行修正的一种方式。对于刚接触的人而言, 最需要的是一个实际使用的实例分析。本文以 android 代码中的 9DOF 算法为例,从卡尔曼 的基本原理,到非线性卡尔曼滤波方程,最后到 android 实际使用的 Indirect Kalman filter。 让大家能够看明白卡尔曼滤波是如何在实际工程中使用的。 整文分两部分。第一部分为理论。重点把结果罗列了一下。本文不是教材,所以推导过程都 省略了。第二部分是代码。代码有些奇怪,用 scilab 写的,而不是 matlab。scilab 是开源的 类 matlab 的工具,对业余爱好者足够。语法也基本类似。 文中的公式都标明了出处。我尽可能避免公式写错。如果有怀疑,可以和后面的代码对照。 最后,如果有问题需要探讨,可以联系:bigstone1998@sina.com。不过回复可能很不及时。

2. Kalman Filter theory
2.1. General
Kalman filter 首先是离散线性系统的 kalman filter。下面就有。 接下去是连续线性系统的 kalman filter。这个把系统离散化就可以。 下面公式来自 Wiki。大家从教材或网上都可以查到。

物理意义: 特别说明一下 P。P 是 covariance of X。例如,如果 X 是 2 维矢量,那么 P 就是一个 2*2 矩 阵。含义是协方差。 启动 Kalman 算法的时候,需要初始值。初始值应该理解为X |?1 , |?1 。因为它们都是非 修正值。 以 g=1 的星球上的自由落体为例, 来说明 Kalman Filter 的物理意义。 这个问题可以描述为: 我们大致知道在 g=1 的星球上,一个物体在 100 米左右,可能有初速度 1 以内的位置开始 下落。每 1 秒钟我们有机会对这个物体进行一次观测,但是观测的精度也是在 1 米以内。看 看如果用 Kalman Filter 方式来修正我们的对物体运动的预测。 物理规律大家都知道: = ? => () = 0 ? ( ? 0) = 0 + 0 ? 0 ? ( ? 0)2 2

把这个方程离散化,假设以 1s 为步长: +1 = + ? /2 +1 = ? 以位置和速度为状态参量 X=[y,v]’,可以得到: X k+1 = 1 1 0.5 X k + ? 0 1 1

从这个状态方程可得到 F,B,u。 然后看测量方程。测量量是位置。位置用状态参量表示为: Z k = 1 0 + () 从测量方程可以得到 H。至于测量的误差量 W(k),根据假设都是 1 米。所以 R=1。观测值 为: 状态参量可以认为没有噪声,所以 Q=0。 初始位置估计为 95 米,初始速度估计为 1。初始的状态变量的估计偏差位置为 10 米,速度 为 1 米/秒。所以初始值为:[100,97.9,94.4,92.7,87.3]。 0|?1 = 这样,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];

10 0 95 ; 0|?1 = 0 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 里面的位置值。 另外,如果设置不同的初始值和初始估计误差,结果会有些不同,但是趋势一样。特别是初 始估计误差。如果设置不合理,比如设置为全 0,那么 Kalman 滤波的结果就会收敛的慢一 些。 所以初值的正确估计和合理设置也很重要。 注意, 和 Q 的初始化, P 必定有一个不为 0。 如果全为 0,就没有必要 Kalman 去修正了。

2.2. 非线性的卡尔曼滤波方程
但是不幸的是,我们常遇到的是非线性的连续系统,如我们的 attitude estimation,就是一个 非线性的应用。 (参考: 《卡尔曼滤波和导航.pdf》P175 $6.2) 通常叫广义卡尔曼滤波(EKF)。 推导就省去了, 教材都有, 结论如下: 对于如下非线性系统: = , + + Z = , +

其中,u(t)是控制信号,w(t)是噪声,v(t)是测量噪声。 下面把 x(k/k),P(k/k)等直接用 x(k),P(k)来代表。 初值准备 首先设置初值 x(0), P(0)。P(0)的设置有两种考虑:第一种,设置的大一些。对于 X(0)估计很 不准的时候可以这样。直接设置为一个大值的对角矩阵 aI。 (太大会对后面计算有影响) 。第 二种,如果 x(0)估计基本靠谱,可以设置 P(0)为 0 矩阵。反正 Kalman 算法是收敛的,初值 的设置只是影响刚开始的收敛速度。 一步转移准备 然后计算干扰转移矩阵Φ (k/k-1): 1 ( ?1 , ?1 ) 1 ( ?1 , ?1 ) ? 1 (?1 ) (?1 ) ? = + ? ? ? ? ( ?1 , ?1 ) ( ?1 , ?1 ) ? 1 (?1 ) (?1 )

?/?1 ? + ?1

?1 = ?1

其中,?1 表示第 k-1 个修正后的 X 值。 计算噪声矩阵:Q(k-1)。假设: = 0; = ( ? ),那么: ? 其中,假设在一个滤波周期 T(T=t(k)-t(k-1))中,G(t)变化不大,Gk=G(tk)。如果发现 Q 的精 度不够,可以采用更高阶的近似,这里就不多说了。 一步转移计算: /?1 = ?1 + ?1 , ?1 + ?1 (?1 ) P/?1 = ?/?1 P?1 ? /?1 + ?1 修正量的准备 测量雅各比矩阵。注意下标的含义。k/k-1 代表用 k-1 理论推算到 k 时刻的值,还未修正。 : [ , ] 1 ( , ) 1 ( , ) ? 1 ( ) ( ) = ? ? ? ( , ) ( , ) ? 1 ( ) ( )

=

? /?1

? /?1

测量噪声矩阵(如果简化,可以设置为 0) : = ( ? ) 滤波系数计算: = /?1 [ /?1 + ]?1 状态变量修正计算(用测量值和未修正的当前状态变量的理论推测值的差来修正): = ? [/?1 , ] = /?1 + 状态变量相关误差矩阵修正: = ( ? ) /?1 ( ? ) + 在实际实践中,可能有多个测量量进行修正。Kalman Filter 的优势就是,如果有可修正的测

量量, 就可以进行修正。 如果没有, 就可以忽略修正过程。 如果有多个修正量, 那么要小心。 - 如果多个修正量是线性无关的,那么它们可以采用连续修正的方式对 X 和 P 进行修正。 - 如果多个修正量是线性相关的,那么情况就复杂了。这里不做描述。最极端情况是,如果 在一次滤波过程中,连续两次用同一个测量量进行修正操作的话,算法会疯掉。当然,正常 人不会这么做,但是难保你的代码不会出现这种问题。

2.3. 9DOF 算法一:严格的非线性 KalmanFilter
前面我们已经有了非线性 Kalman 的理论,下面可以把它用于 9DOF 的 altitude 的计算上。 参考:[Kalman Filtering For Spacecraft Attitude Estimation, E.J. Lefferts] 在往下走之前,特别强调几点: 1, 这里用的 quanternion,和教材上的 Hamilton 定义的有些不一样。区别在于:quanterion 的 定 义 都 是一 致 的 。这表 明 从 quanternion 到方向 余 弦 的 算法 是 一 致的。 但 是 对 quanternion 乘法的定义不一样。 Hamilton 的定义是 ij= - ji=k, 而这里的定义是 ij= -ji = -k。 这样定义有这样定义的好处,特别是用 quanternion 进行运算的时候,会方便些。这种乘 法定义,和 Wiki 上的也是不同的,需要特别小心。 2, 关于 quanternion 的定义,还需要注意特别分量 cos(θ/2)的位置。这里用的都是放在第四 位。 3, 在 quanternion 和方向余弦转换的时候,特别注意方向余弦是从 reference 到 body,还是 相反。两者之间是互逆或转置关系。 特别写出下面用到的 quanternion 的一些算法。不然,如果没搞明白,随便参考教材看代码 的话,会晕过去的。为避免笔误,直接上图:

首先要依照物理规律写出状态方程和测量修正方程。推导过程忽略,结论如下: 1 = 2Ω ? ? 2Ξ(())1 ()
1

= 2 () 其中, 为 quanternion, 是载体上的 gyroscope 的输出。 是 bias。 1 是角速度的测量噪声, q u b η 满足: = ? ? 1 。而η2 代表 bias 偏差的噪声。 同样, 用上三角表示预测量, 并取[q,b]为状态参量, 这样一步转移(Prediction)的状态方程为: 1 = 2Ω ; = 0 其中 = ? 是一步转移(estimated)角速度。从这个方程离散化后得到状态转移方程: Δ 2 sin
Δ 2

+ 1/ =

cos

4×4 +

Δ

Ω Δ

;

Δ = ΔT = ( ? )ΔT + 1/ = 其中,状态变量为[q,b]’。 下面计算 P 矩阵:F,G,q, 根据上一节的定义,得到: 1 1 1 Ω(()) ? Ξ(()) ; = ? Ξ(()) 04×3 = 2 2 2 03×4 03×3 03×3 3×3 = 1 ()1 () 1 => = 2 03×3


03×3 2 ()2 ()


=> ? Δ + Δ

P+1/ = ?+1/ P ? +1/ + = + Δ P + Δ

顺便说一下初始值设置。初始值可以根据重力和磁场来计算初始 q。这样初始的 P 可以设置 为 0 矩阵。 关于噪声的说明: 1 ()1 () 就是角速度的测量方差, 从器件手册上可以得到, 例如 1e-7。 2 ()2 () 就是偏差漂移量方差,例如 1e-8。都是以 rad 来计算。 接下去就是修正方程。这里采用角速度和磁场同时修正的方式。也可以一个一个来,在下面 的例子中描述。 如果同时以 A/M 为测量修正量,那么测试量就是一个 6 维向量: = ? ; = 0,0,1 ′ ; = [0,1,0]′ ? 这里的全局坐标系以东为 X,北为 Y,向上为 Z。 计算 H 矩阵(参考[Kalman-Filter-Based Orientation Determination Using Inertial/Magnetic Sensor: Observability Analysis and Performance Evaluation, Angelo Maria Sabatini ]) :


特别注意余弦矩阵的方向,是从全局(大地)坐标转换到传感器坐标。 +1 = [ +1 , +1 ] +1 =
+1 ? +1/ Ψ(+1/ , ) 03×3 Ψ(+1/ , ) 03×3

H 矩阵是一个 7X6 的矩阵。 接下去的操作就是计算 K,然后用测量的重力和北向来和 q(k+1/k)计算得到的 DCM 旋转过 来的重力北向差来计算修正量,并对 q 和 b 进行修正。如上一节所描述。 这个算法有一个问题:状态变量 X 由 quanternion q 和角速度 bias 构成。修正量 dq 是代数相 加到 q(k+1/k)上面去的。 quanternion 需要保持|q|=1。 而 所以需要对最后修正后的 q 进行调整: - 归一化。就是 q/|q|。 - 检查 q4 是否大于 0。如果小于 0,那么 q= - q,进行调整。这利用到 quanternion 的一个 特点: 和-q 其实代表一个结果相同的转动。 q 只是当 q4 大于 0 的那个转动所需要转动的 角度最小。 为了避免这个问题,于是又产生了下面的 Indirect Kalman Filter 算法。

2.4. Indirect Kalman Filter for 3D Attitude Estimation
参考:[Indirect Kalman Filter for 3D Attitude Estimation, Nikolas Trawny and Stergios I. Roumeliotis] Android 中的 9DOF 算法就是用的这种算法。 依然采用上节相同的 quanternion 乘法定义。这里面有时候能够看到 的这种表示,含义就 是表示这个 q 代表以 G 坐标为基准,向 L 坐标转动的 q。其实和上一节的 q 含义一样。

首先做一个不痛不痒的改进。从 q(k-1)计算 q(k/k-1)的时候,用更高阶的近似: 令:Θ(ω, Δt) = cos
+1 + 2 Δt 2

4×4 +

sin

Δ t 2

Δt

Ω Δt

取: =

+ 1/ = Θ ω, Δt +

Δ 2 (Ω +1 Ω ? Ω Ω +1 ) ; 48

接下去对修正量定义进行了修改。不是 quanternion 的代数修正,而是旋转修正:

由于修正量 dq 通常很小,所以有:δq = 于是定义状态变量修正量为:δX = Δ 根据这个定义,得到的相关矩阵为:

sin(/2) ? /2 cos(/2) 1

所以,给定初值后,可以用ω( + 1/) = ( + 1) ? ( + 1/) = ( + 1) ? (),然后 进行计算 X(k+1/k)和 P(k+1/k)。 注意,这里的 X 定义依然是[q,b]’。和 dx 的定义有区别。这就是叫 Indirect Kalman filter 的 原因。
然后计算修正量矩阵 H。H = ( (+1/ ) ? ) × 03×3 注意,这里我们只用了一个测量量来计算 H。其实我们有两个,向上和向北。由于这两个测 量量代表不相关的修正, 所以它们可以用来连续进行进行修正。 在后面代码中两种修正方式 都有。如果同时修正,那么 H 就是 6x6 矩阵。如果每次用一个,H 就是 3x3 矩阵。 令测量误差为 0,也就是 R=0。可以得到δX。从δX恢复 dq 如下:

3. Scilab 代码
代码说明 代码用 scilab 编写。有 matlab 基础看起来也很方便。另外,这个代码很“脏” 。也就是说很 多调试代码都在里面。我也不想清理。一清理,又要对代码的正确性进行验证。 有前面的理论公式,这里对代码就不多注释。基本架构就是从文件中读取 sensor 的数据, 然后对不同的 sensor 进行处理。如果是 gyroscop,就进行下一步预测;如果是 accelerator 或 magnetic field,就进行修正。前面提到的 A/M 同时修正和分别修正在下面的代码里都有 体现。 代码中有些奇怪的 if 语句,其实是用来控制不同的卡尔曼滤波实现测试的。想用不同算法 的时候,就调整 if 语句的条件。相信看看就能明白。 代码中还有很多垃圾语句,看的时候不要被它们牵扯太多精力。 另外代码拷贝到 word 中,有可能被 word“智能”修改了些东西,如果大小写,减号啥的, 也要注意。 最后说明一下,Kalman filter 的工程实现的时候,k,k+1 什么的很容易搞混头。所以我下面 的代码也未必完全正确。但是我能保证的是,基本能用。

3.1. 卡尔曼滤波修正 gyro bias 和计算方向余弦

stacksize('max'); Longi=[];Latt=[]; Latt(1)=31.142057;Longi(1)=121.388764;// longitude and lattitude Height=[]; Height(1)=0;// just move on the ground.

// init AHRS RPhi=[];PSita=[];HKsai=[]; RPhi(1)=-1.149*%pi/180;// -1.0245667roll, along X, North, rad, 3rd PSita(1)=0.8*%pi/180;// 0 Pitch, along Y, East, rad, 2nd HKsai(1)=-93*%pi/180;// 1.1706848 Heading, along z, D, rad, 1st // init velocity VN=[];VE=[];VD=[]; VN(1)=0;VE(1)=0;VD(1)=0; // Data soure file name strFile="static.csv";

////////////////////////////////////////// // function change from Euler angle to quaternion functionqua_vec=eulr2qua(eul_vect) // // // // // // // // // // // // // // INPUTS eul_vect(1) = roll angle in radians eul_vect(2) = pitch angle in radians eul_vect(3) = heading angle in radians OUTPUTS qua_vec = 4 element quaternion vector = [a b c d] where: a = cos(MU/2) b = (MUx/MU)*sin(MU/2) c = (MUy/MU)*sin(MU/2) d = (MUz/MU)*sin(MU/2) where: MUx, MUy, MUz are the components of the angle vector MU is the magnitude of the angle vector qua_vec = eulr2qua(eul_vect)

phi=eul_vect(1);theta=eul_vect(2);psi=eul_vect(3);

cpsi2=cos(psi/2);spsi2=sin(psi/2); cthe2=cos(theta/2);sthe2=sin(theta/2); cphi2=cos(phi/2);sphi2=sin(phi/2);

a=cphi2*cthe2*cpsi2+sphi2*sthe2*spsi2; b=sphi2*cthe2*cpsi2-cphi2*sthe2*spsi2; c=cphi2*sthe2*cpsi2+sphi2*cthe2*spsi2; d=cphi2*cthe2*spsi2+sphi2*sthe2*cpsi2;

qua_vec=[abcd]; endfunction

// QUA2DCM

Quaternion to direction cosine matrix conversion.

// This is b to n. If n to b needed, just transefer it.

functionDCMbn=qua2dcm(qua_vec) // // // // // // // // // // // // // OUTPUT DCMbn = 3x3 direction cosine matrix providing the transformation from the body frame to the navigation frame INPUT qua_vec = 4 element quaternion vector = [a b c d] where: a = cos(MU/2) b = (MUx/MU)*sin(MU/2) c = (MUy/MU)*sin(MU/2) d = (MUz/MU)*sin(MU/2) where: MUx, MUy, MUz are the components of the angle vector MU is the magnitude of the angle vector

a=qua_vec(1);b=qua_vec(2);c=qua_vec(3);d=qua_vec(4);

DCMbn(1,1)=a*a+b*b-c*c-d*d; DCMbn(1,2)=2*(b*c-a*d); DCMbn(1,3)=2*(b*d+a*c); DCMbn(2,1)=2*(b*c+a*d); DCMbn(2,2)=a*a-b*b+c*c-d*d; DCMbn(2,3)=2*(c*d-a*b); DCMbn(3,1)=2*(b*d-a*c); DCMbn(3,2)=2*(c*d+a*b); DCMbn(3,3)=a*a-b*b-c*c+d*d; endfunction

///////////////////////////////////////////////////////////////////// //全局变量 globalglv glv.Re=6378160;//地球半径 glv.f=1/298.3;//地球扁率 glv.e=sqrt(2*glv.f-glv.f^2);glv.e2=glv.e^2;//地球椭圆度等其它几何参数 glv.Rp=(1-glv.f)*glv.Re; glv.ep=sqrt(glv.Re^2+glv.Rp^2)/glv.Rp;glv.ep2=glv.ep^2; glv.wie=7.2921151467e-5;//地球自转角速率 glv.g0=0;// 9.7803267714; //重力加速度 glv.mg=1.0e-3*glv.g0;//毫重力加速度 glv.ug=1.0e-6*glv.g0;//微重力加速度 glv.ppm=1.0e-6;//百万分之一 glv.deg=%pi/180;//角度 glv.min=glv.deg/60;//角分 glv.sec=glv.min/60;//角秒 glv.hur=3600;//小时

glv.dph=glv.deg/glv.hur;//度/小时 glv.mil=2*%pi/6000;//密位 glv.nm=1853;//海里 glv.kn=glv.nm/glv.hur;//节 glv.cs=[//圆锥和划船误差补偿系数 2.0/3,0,0,0 9.0/20,27.0/20,0,0 54.0/105,92.0/105,214.0/105,0 250.0/504,525.0/504,650.0/504,1375.0/504];

////////////////////////////////////////////////// // load acceleration and gyro data TYPEACC=1; TYPEGYRO=4; TYPEROT=2; TYPEORI=10; BMULTITYPE=%t; SensorType=[TYPEACC,TYPEGYRO,TYPEROT,TYPEORI]; SensorIndex=[0,0,0,0,0];// for mutliple sensor instance of the same type SensorTypeIndex=4;// column index in the log file MultiTypeIndex=10; // end of global parameter // output sensor data is in SensorDataLst. SensorDataLst=list(); fori=1:length(SensorType) SensorDataLst(i)=[]; end ////////////////////////////////////////////

u=file('open',strFile,'old'); // discard the first line Ah=read(u,1,1,'(a)'); // read all line, and first 7 column. In scilab, index start from 1. MA=[]; MA=read(u,-1,7); ifBMULTITYPEthen// read the sensor type index fsize=size(MA); SensorInstIndex=csvRead(strFile,[],[],[],[],[],[2,MultiTypeIndex,fsize(1)+1,MultiTypeIndex]); end file('close',u); // adjust the time, cut the uesless large part //t_temp=MA(1,3)-pmodulo(MA(1,3),1e6); //MA(:,3)=MA(:,3)-t_temp; //t_temp=MA(1,2)-pmodulo(MA(1,2),1e6);

//MA(:,2)=MA(:,2)-t_temp;

[SRow,SCol]=size(MA); AccNumRow=1;GyrNumRow=1;RotNumRow=1;OriNumRow=1;

// The acceleration and gyroscope data must be matched. Acc will read first, // then gyroscope data will be waited. fbIn=[];wbIn=[];srcROT=[];srcORI=[]; fori=1:SRow if(TYPEACC==MA(i,SensorTypeIndex))then t_bool=1; // check subtype ifBMULTITYPEthen ifSensorIndex(1)<>SensorInstIndex(i)then t_bool=0; end end if(1==t_bool)then fbIn(AccNumRow,1)=MA(i,2)/1.0e3;// timemark in event, convert to second fbIn(AccNumRow,2:4)=[MA(i,5),MA(i,6),MA(i,7)]; AccNumRow=AccNumRow+1; continue; end end if(TYPEGYRO==MA(i,SensorTypeIndex))then t_bool=1; // check subtype ifBMULTITYPEthen ifSensorIndex(2)<>SensorInstIndex(i)then t_bool=0; end end if(1==t_bool)then wbIn(GyrNumRow,1)=MA(i,2)/1.0e3;// timemark in event, convert to second wbIn(GyrNumRow,2:4)=[MA(i,5),MA(i,6),MA(i,7)]; GyrNumRow=GyrNumRow+1; continue end end if(TYPEROT==MA(i,SensorTypeIndex))then t_bool=1; // check subtype ifBMULTITYPEthen ifSensorIndex(3)<>SensorInstIndex(i)then

t_bool=0; end end if(1==t_bool)then srcROT(RotNumRow,:)=[MA(i,5),MA(i,6),MA(i,7)]; RotNumRow=RotNumRow+1; continue; end end if(TYPEORI==MA(i,SensorTypeIndex))then t_bool=1; // check subtype ifBMULTITYPEthen ifSensorIndex(4)<>SensorInstIndex(i)then t_bool=0; end end if(1==t_bool)then srcORI(OriNumRow,:)=[MA(i,5),MA(i,6),MA(i,7)]; OriNumRow=OriNumRow+1; continue; end end end AccNumRow=AccNumRow-1; GyrNumRow=GyrNumRow-1; NumRow=min(AccNumRow,GyrNumRow);

// check p-p value, based on linear acc value MaxAcc=max(fbIn(:,2:4)); MinAcc=min(fbIn(:,2:4)); ThAcc=(MaxAcc-MinAcc)/20; MaxGry=max(wbIn(:,2:4)); MinGry=min(wbIn(:,2:4)); ThGry=(MaxGry-MinGry)/20;

// calculate the navigation data RPhi=[]; PSita=[]; HKsai=[]; qua_vec=[]; qua_vec(1,:)=eulr2qua([RPhi(1),PSita(1),HKsai(1)])// init quaternion XN=[];XE=[];XD=[];CalcAcc=[]; XN(1)=0;XE(1)=0;XD(1)=0; ///// Extended Kalman /////////////////////////////////////////////// // helper function // generate ax matrics. The input vector is 3 dimension

// function below is based on q4 is standalone in quanterion functionrv=VectorCross(arg_vec) rv(1,1)=0; rv(2,2)=0; rv(3,3)=0; rv(1,2)=-arg_vec(3); rv(1,3)=arg_vec(2); rv(2,1)=arg_vec(3); rv(2,3)=-arg_vec(1); rv(3,1)=-arg_vec(2); rv(3,2)=arg_vec(1); endfunction

// vector cross product functionrv=CrossProduct(u, v) rv(1)=u(2)*v(3)-u(3)*v(2); rv(2)=u(3)*v(1)-u(1)*v(3); rv(3)=u(1)*v(2)-u(2)*v(1); endfunction

// quanternion product P=a*b // the 1st one is the standalone one. q4 is different. // And special note: this multiplication is JPL convention, not Hamilton notation!!!!!!! // that is ij=-ji=-k functionP=QuaternionProduct(a, b) a1=a(1); a2=a(2); a3=a(3); a4=a(4);

b1=b(1); b2=b(2); b3=b(3); b4=b(4);

P1=a4*b1+a3*b2-a2*b3+a1*b4; P2=-a3*b1+a4*b2+a1*b3+a2*b4; P3=a2*b1-a1*b2+a4*b3+a3*b4; P4=-a1*b1-a2*b2-a3*b3+a4*b4; P=[P1;P2;P3;P4]; endfunction // quanternion product P=a*b // the 1st one is the standalone one. q1 is different. // this is Hamilton multiplication, that is: ij=-ji=k

functionP=HTQuaternionProduct(a, b) a1=a(1); a2=a(2); a3=a(3); a4=a(4);

b1=b(1); b2=b(2); b3=b(3); b4=b(4);

P1=a1*b1-a2*b2-a3*b3-a4*b4; P2=a1*b2+a2*b1+a3*b4-a4*b3; P3=a1*b3-a2*b4+a3*b1+a4*b2; P4=a1*b4+a2*b3-a3*b2+a4*b1; P=[P1;P2;P3;P4]; endfunction

functionrv=Omiga(w) rv(1,1)=0;rv(1,2)=w(3);rv(1,3)=-w(2);rv(1,4)=w(1); rv(2,1)=-w(3);rv(2,2)=0;rv(2,3)=w(1);rv(2,4)=w(2); rv(3,1)=w(2);rv(3,2)=-w(1);rv(3,3)=0;rv(3,4)=w(3); rv(4,1)=-w(1);rv(4,2)=-w(2);rv(4,3)=-w(3);rv(4,4)=0; endfunction

functionrv=E3(q) rv(1,1)=q(4);rv(1,2)=-q(3);rv(1,3)=q(2); rv(2,1)=q(3);rv(2,2)=q(4);rv(2,3)=-q(1); rv(3,1)=-q(2);rv(3,2)=q(1);rv(3,3)=q(4); rv(4,1)=-q(1);rv(4,2)=-q(2);rv(4,3)=-q(3); endfunction

// init value // ignore the Q and R. Kq=[];Kb=[];KP=[];KK=[];dx=[]// KK is 7x6 Kq(1,1:3)=qua_vec(1,2:4);Kq(1,4)=qua_vec(1,1);// in Kq, q4 is standalone Kb(1,:)=[0.0025,0.0025,0.0025]; KP(1,:,:)=zeros(7,7);// covariance erro. Init to all 0 // Kp init to zero, then Q can't be zero, must be inited. gyroVar=1e-7;biasVar=1e-8;// var, already squared. Q=zeros(6,6); Q(1,1)=gyroVar;Q(2,2)=gyroVar;Q(3,3)=gyroVar; Q(4,4)=biasVar;Q(5,5)=biasVar;Q(6,6)=biasVar; dx(1,:)=zeros(1,7);

wbUpdated=[];// updated wb

fori=1:0//NumRow-1 DT=wbIn(i+1,1)-wbIn(i,1); if(1e-6>DT)thenDT=1e-6;end;

// prediction///////////////////////////////////// wb=wbIn(i,2:4)-Kb(i,:); wbUpdated(i,:)=wb; Dsita=wb*DT; DsitaL=norm(Dsita); MDSita=cos(DsitaL/2)*eye(4,4)+sin(DsitaL/2)*Omiga(Dsita)/DsitaL; Kq(i+1,:)=(MDSita*(Kq(i,:)'))';// based on q4 is different Kb(i+1,:)=Kb(i,:);

// update///////////////////////////////////////// // calculate Fk and Gk tempOmiga=Omiga(wb); tempE3=E3(Kq(i,:));// use old updated value to calculate KF=zeros(7,7); forj=1:4 fork=1:4 KF(j,k)=tempOmiga(j,k)/2.0; end end forj=1:4 fork=1:3 KF(j,k+4)=-tempE3(j,k)/2.0; end end

KG=zeros(7,6); forj=1:4 fork=1:3 KG(j,k)=-tempE3(j,k)/2.0; end end KG(5,4)=1;KG(6,5)=1;KG(7,6)=1;

// calc Phi and GQGt Phi=eye(7,7)+KF*DT; GQGT=KG*Q*(KG')*DT;// 7x7 matrix

// calc temp P

tempP=Phi*matrix(KP(i,:,:),7,7)*(Phi')+GQGT;

// calc H. Here up and north will be used as Z. // For the equation below, the Z is in body system. // H is a 6x7 matrix, H=[L 0(6x3)]. L is a 6x4 matrix. // half of H is [2q4[px]+2([px][qx]-2[qx][px]) -2[qx]p] // use update q to calculate the H // For up: [0,0,1], note the position of q4: // calc measured Z. Z=[up north]' Mup=fbIn(i,2:4)/norm(fbIn(i,2:4)); Mmg=srcROT(i,:)/norm(srcROT(i,:)); Mn=(CrossProduct(Mup,CrossProduct(Mmg,Mup)))'; Mn=Mn/norm(Mn);

// use current Xk/k-1 for H calc Sup=[0,0,1]; Lup=2*Kq(i+1,4)*VectorCross(Sup)+2*(VectorCross(Sup)*VectorCross(Kq(i+1,1:3))-2*VectorCross(Kq(i+1,1:3)*VectorCross(Sup))); tempM=-2*VectorCross(Kq(i+1,1:3))*Sup'; Lup(1,4)=tempM(1);Lup(2,4)=tempM(2);Lup(3,4)=tempM(3); Sn=[0,1,0]; Ln=2*Kq(i+1,4)*VectorCross(Sn)+2*(VectorCross(Sn)*VectorCross(Kq(i+1,1:3))-2*VectorCross(Kq(i+1,1:3)*VectorCross(Sn))); tempM=-2*VectorCross(Kq(i+1,1:3))*Sn'; Ln(1,4)=tempM(1);Ln(2,4)=tempM(2);Ln(3,4)=tempM(3);

KH=zeros(6,7); forj=1:3 fork=1:4 KH(j,k)=Lup(j,k); KH(j+3,k)=Ln(j,k); end end

// calc KK. Assume R=0, no error in measurement result. Just believe it. KK(i,:,:)=tempP*(KH')/(KH*tempP*(KH')); // calc KP(i+1) and dq. KK is 7x6 KP(i+1,:,:)=(eye(7,7)-matrix(KK(i,:,:),7,6)*KH)*tempP*((eye(7,7)-matrix(KK(i,:,:),7,6)*KH)');

// calc theory up and north in DCMbn=qua2dcm([Kq(i+1,4)Kq(i+1,1:3)]); DCMnb=DCMbn'; Cup=DCMnb*(Sup');// will compare with [0,0,1] Cn=DCMnb*(Sn');// will compare with [0,1,0]

// Calc dx delta=zeros(1,6); delta(1)=Mup(1)-Cup(1);delta(2)=Mup(2)-Cup(2);delta(3)=Mup(3)-Cup(3); delta(4)=Sn(1)-Cn(1);delta(5)=Sn(2)-Cn(2);delta(6)=Sn(3)-Cn(3); dx(i,:)=(matrix(KK(i,:,:),7,6)*delta')'; // adjust q and b. + or -, depends on the dz, not a big issue, bug need take care of it. Kq(i+1,:)=Kq(i+1,:)+dx(i,1:4); Kq(i+1,:)=Kq(i+1,:)/norm(Kq(i+1,:)); if(Kq(i+1,4)<0)then Kq(i+1,:)=-Kq(i+1,:) end Kb(i+1,:)=Kb(i+1,:)+dx(i,5:7);

end

////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////// //////// Indirect Kalman Filter ///////////////////////// //////////////////////////////////////////////////////////// // init value // ignore the Q and R. IKq=[];IKb=[];IKP=[];IKK=[];Idx=[];// IKK is 6x6 IKq(1,1:3)=qua_vec(1,2:4);IKq(1,4)=qua_vec(1,1);// in Kq, q4 is standalone IKb(1,:)=[0.0025,0.0025,0.0025]; IKP(1,:,:)=zeros(6,6);// covariance erro. Init to all 0 // Kp init to zero, then Q can't be zero, must be inited. gyroVar=1e-7;biasVar=1e-8;// var, already squared. DT=0.010;// 10 ms IGQGT=zeros(6,6); IGQGT(1:3,1:3)=gyroVar*DT*eye(3,3); IGQGT(4:6,4:6)=biasVar*DT*eye(3,3); IGQGT(4:6,1:3)=-0.5*biasVar*DT*DT*eye(3,3); IGQGT(1:3,4:6)=IGQGT(4:6,1:3); Idx(1,:)=zeros(1,6); IwbUpdated=[];// updated wb IwbUpdated(1,:)=wbIn(1,2:4);//+[0.03,-0.03,0.06]; Idelta=[]; fori=1:NumRow-1 // prediction///////////////////////// IKb(i+1,:)=IKb(i,:);// b(k+1/k) IwbUpdated(i+1,:)=wbIn(i+1,2:4)-IKb(i+1,:);//+[0.03,-0.03,0.06]; // w(k+1/k)

// q propagation equation. First order quaternion integrator wb=(IwbUpdated(i+1,:)+IwbUpdated(i,:))/2.0;// (w(k+1/k)+w(k/k))/2

Dsita=wb*DT; DsitaL=norm(Dsita); MDSita=cos(DsitaL/2)*eye(4,4)+sin(DsitaL/2)*Omiga(Dsita)/DsitaL; wbk1=IwbUpdated(i+1,:);wbk=IwbUpdated(i,:); MDSita=MDSita+(Omiga(wbk1)*Omiga(wbk)-Omiga(wbk)*Omiga(wbk1))*DT*DT/48.0; // // // // wb=IwbUpdated(i,:); Dsita=wb*DT; DsitaL=norm(Dsita); MDSita=cos(DsitaL/2)*eye(4,4)+sin(DsitaL/2)*Omiga(Dsita)/DsitaL;

IKq(i+1,:)=(MDSita*(IKq(i,:)'))';// based on q4 is different, and q(k+1/k) // P prediction wb=IwbUpdated(i,:);// use w(k/k) wbl=norm(wb); Psi=eye(6,6); t_tv=(1-cos(wbl*DT))/(wbl*wbl); Psi(1:3,1:3)=eye(3,3)-sin(wbl*DT)*VectorCross(wb)/wbl+t_tv*VectorCross(wb)*VectorCross(wb); Psi(1:3,4:6)=-eye(3,3)*DT+t_tv*VectorCross(wb)-(wbl*DT-sin(wbl*DT))*VectorCross(wb)*VectorCross(wb)/(wbl^3); IKP(i+1,:,:)=Psi*matrix(IKP(i,:,:),6,6)*(Psi')+IGQGT;

// update // try every 4 sample // if(modulo(i,4)<>0) then continue;end;

// calc H. Will use up and north vector.Use X(k/k-1) for H calculation IKH=zeros(6,6); Sup=[0,0,1];Sn=[0,1,0]; DCMbn=qua2dcm([IKq(i+1,4)IKq(i+1,1:3)]); DCMnb=DCMbn'; Cup=DCMnb*(Sup');// will compare with [0,0,1] Cn=DCMnb*(Sn');// will compare with [0,1,0] IKH(1:3,1:3)=VectorCross(Cup); IKH(4:6,1:3)=VectorCross(Cn); // assume R=0 t_tv=[]; t_tv=matrix(IKP(i+1,:,:),6,6); IKK(i,:,:)=t_tv*(IKH')/(IKH*t_tv*(IKH')); // dx calc Mup=fbIn(i,2:4)/norm(fbIn(i,2:4)); Mmg=srcROT(i,:)/norm(srcROT(i,:)); Mn=(CrossProduct(Mup,CrossProduct(Mmg,Mup)))'; Mn=Mn/norm(Mn); delta=zeros(1,6); delta(1:3)=Mup-Cup'; delta(4:6)=Mn-Cn'; Idelta(i,:)=delta;

Idx(i,:)=(matrix(IKK(i,:,:),6,6)*delta')'; // update X and P. Calc dq t_tv=[];t_tv=Idx(i,1:3)/2.0; ift_tv*t_tv'<1then dq=[t_tv,sqrt(1-t_tv*t_tv')]; else dq=[t_tv,1]/sqrt(1+t_tv*t_tv'); end // // t_tv=[];t_tv=QuaternionProduct([dq(4),dq(1:3)],[IKq(i+1,4),IKq(i+1,1:3)])'; IKq(i+1,:)=[t_tv(2:4)',t_tv(1)];

IKq(i+1,:)=QuaternionProduct(dq,IKq(i+1,:))'; IKb(i+1,:)=IKb(i+1,:)+Idx(i,4:6); // IwbUpdated(i+1,:)=wbIn(i+1,2:4)-IKb(i+1,:)+[0.03,-0.03,0.06];// w(k+1/k+1)

IwbUpdated(i+1,:)=IwbUpdated(i+1,:)-Idx(i,4:6); t_tv=[];t_tv=matrix(IKK(i,:,:),6,6); IKP(i+1,:,:)=(eye(6,6)-t_tv*IKH)*matrix(IKP(i+1,:,:),6,6)*((eye(6,6)-t_tv*IKH)'); end

3.2. Kalman 滤波计算线性加速度
Android 中的算法基本上就是这个算法。
//////////////////////////////////////////////////// // init value stacksize('max'); // Data soure file name strFile="Right.csv"; DT=0.01;// ms InitNum=50;// 50 point to init, depends on the DT typeIndex=4;dataPos=5;timePos=2;MultiTypeIndex=10; TYPEACC=1;TYPEMAG=2;TYPEGYRO=4;TYPELACC=10; SUBTYPEACC=0;SUBTYPEMAG=0;SUBTYPEGYRO=0;SUBTYPELACC=0;// instance index of each sensor type MaxMag=100; aUpdateThred=0.3/DT;// 300 ms aUpdateVar=0.1;// 10% error is allowed 9.8*0.9~9.8*1.1

////////////////////////////////////////// // function change from Euler angle to quaternion // Note: all quaternion are from global to boday rotation. functionqua_vec=eulr2qua(eul_vect) // // // // INPUTS eul_vect(1) = roll angle in radians eul_vect(2) = pitch angle in radians qua_vec = eulr2qua(eul_vect)

// // // // // // // // // //

eul_vect(3) = heading angle in radians OUTPUTS qua_vec = 4 element quaternion vector = [a b c d] where: a = cos(MU/2) b = (MUx/MU)*sin(MU/2) c = (MUy/MU)*sin(MU/2) d = (MUz/MU)*sin(MU/2) where: MUx, MUy, MUz are the components of the angle vector MU is the magnitude of the angle vector

phi=eul_vect(1);theta=eul_vect(2);psi=eul_vect(3);

cpsi2=cos(psi/2);spsi2=sin(psi/2); cthe2=cos(theta/2);sthe2=sin(theta/2); cphi2=cos(phi/2);sphi2=sin(phi/2);

a=cphi2*cthe2*cpsi2+sphi2*sthe2*spsi2; b=sphi2*cthe2*cpsi2-cphi2*sthe2*spsi2; c=cphi2*sthe2*cpsi2+sphi2*cthe2*spsi2; d=cphi2*cthe2*spsi2+sphi2*sthe2*cpsi2;

qua_vec=[abcd]; endfunction

// QUA2DCM

Quaternion to direction cosine matrix conversion.

// The quanternion is rotate from global to body. // And the result DCM is from body to global. functionDCMbn=qua2dcm(qua_vec) // // // // // // // // // // // // // OUTPUT DCMbn = 3x3 direction cosine matrix providing the transformation from the body frame to the navigation frame INPUT qua_vec = 4 element quaternion vector = [a b c d] where: a = cos(MU/2) b = (MUx/MU)*sin(MU/2) c = (MUy/MU)*sin(MU/2) d = (MUz/MU)*sin(MU/2) where: MUx, MUy, MUz are the components of the angle vector MU is the magnitude of the angle vector

a=qua_vec(1);b=qua_vec(2);c=qua_vec(3);d=qua_vec(4);

DCMbn(1,1)=a*a+b*b-c*c-d*d;

DCMbn(1,2)=2*(b*c-a*d); DCMbn(1,3)=2*(b*d+a*c); DCMbn(2,1)=2*(b*c+a*d); DCMbn(2,2)=a*a-b*b+c*c-d*d; DCMbn(2,3)=2*(c*d-a*b); DCMbn(3,1)=2*(b*d-a*c); DCMbn(3,2)=2*(c*d+a*b); DCMbn(3,3)=a*a-b*b-c*c+d*d; endfunction

// DCM to quanternion (q1 is standalone) // the (i,j) value means the cos between ref i axis and body j axis // The input DCM is from body to global functionq=dcmbn2qua(dcm) q1=0.5*sqrt(1+dcm(1,1)^2+dcm(2,2)^2+dcm(3,3)^2); q2=(dcm(3,2)-dcm(2,3))/(4.0*q1); q3=(dcm(1,3)-dcm(3,1))/(4.0*q1); q4=(dcm(2,1)-dcm(1,2))/(4.0*q1); q=[q1;q2;q3;q4]; q=q/norm(q); endfunction

///// Extended Kalman /////////////////////////////////////////////// // helper function // generate ax matrics. The input vector is 3 dimension // function below is based on q4 is standalone in quanterion functionrv=VectorCross(arg_vec) rv(1,1)=0; rv(2,2)=0; rv(3,3)=0; rv(1,2)=-arg_vec(3); rv(1,3)=arg_vec(2); rv(2,1)=arg_vec(3); rv(2,3)=-arg_vec(1); rv(3,1)=-arg_vec(2); rv(3,2)=arg_vec(1); endfunction

// vector cross product functionrv=CrossProduct(u, v) rv(1)=u(2)*v(3)-u(3)*v(2); rv(2)=u(3)*v(1)-u(1)*v(3); rv(3)=u(1)*v(2)-u(2)*v(1); endfunction

// quanternion product P=a*b // the 1st one is the standalone one. q4 is different. // And special note: this multiplication is JPL convention, not Hamilton notation!!!!!!! // that is ij=-ji=-k functionP=QuaternionProduct(a, b) a1=a(1); a2=a(2); a3=a(3); a4=a(4);

b1=b(1); b2=b(2); b3=b(3); b4=b(4);

P1=a4*b1+a3*b2-a2*b3+a1*b4; P2=-a3*b1+a4*b2+a1*b3+a2*b4; P3=a2*b1-a1*b2+a4*b3+a3*b4; P4=-a1*b1-a2*b2-a3*b3+a4*b4; P=[P1;P2;P3;P4]; endfunction // quanternion product P=a*b // the 1st one is the standalone one. q1 is different. // this is Hamilton multiplication, that is: ij=-ji=k functionP=HTQuaternionProduct(a, b) a1=a(1); a2=a(2); a3=a(3); a4=a(4);

b1=b(1); b2=b(2); b3=b(3); b4=b(4);

P1=a1*b1-a2*b2-a3*b3-a4*b4; P2=a1*b2+a2*b1+a3*b4-a4*b3; P3=a1*b3-a2*b4+a3*b1+a4*b2; P4=a1*b4+a2*b3-a3*b2+a4*b1; P=[P1;P2;P3;P4]; endfunction

functionrv=Omiga(w)

rv(1,1)=0;rv(1,2)=w(3);rv(1,3)=-w(2);rv(1,4)=w(1); rv(2,1)=-w(3);rv(2,2)=0;rv(2,3)=w(1);rv(2,4)=w(2); rv(3,1)=w(2);rv(3,2)=-w(1);rv(3,3)=0;rv(3,4)=w(3); rv(4,1)=-w(1);rv(4,2)=-w(2);rv(4,3)=-w(3);rv(4,4)=0; endfunction

functionrv=E3(q) rv(1,1)=q(4);rv(1,2)=-q(3);rv(1,3)=q(2); rv(2,1)=q(3);rv(2,2)=q(4);rv(2,3)=-q(1); rv(3,1)=-q(2);rv(3,2)=q(1);rv(3,3)=q(4); rv(4,1)=-q(1);rv(4,2)=-q(2);rv(4,3)=-q(3); endfunction ///////////////////////////////////////////////////////////////////// //全局变量 globalglv glv.Re=6378160;//地球半径 glv.f=1/298.3;//地球扁率 glv.e=sqrt(2*glv.f-glv.f^2);glv.e2=glv.e^2;//地球椭圆度等其它几何参数 glv.Rp=(1-glv.f)*glv.Re; glv.ep=sqrt(glv.Re^2+glv.Rp^2)/glv.Rp;glv.ep2=glv.ep^2; glv.wie=7.2921151467e-5;//地球自转角速率 glv.g0=0;// 9.7803267714; //重力加速度 glv.mg=1.0e-3*glv.g0;//毫重力加速度 glv.ug=1.0e-6*glv.g0;//微重力加速度 glv.ppm=1.0e-6;//百万分之一 glv.deg=%pi/180;//角度 glv.min=glv.deg/60;//角分 glv.sec=glv.min/60;//角秒 glv.hur=3600;//小时 glv.dph=glv.deg/glv.hur;//度/小时 glv.mil=2*%pi/6000;//密位 glv.nm=1853;//海里 glv.kn=glv.nm/glv.hur;//节 glv.cs=[//圆锥和划船误差补偿系数 2.0/3,0,0,0 9.0/20,27.0/20,0,0 54.0/105,92.0/105,214.0/105,0 250.0/504,525.0/504,650.0/504,1375.0/504];

///////////////////////////////////////////////////////////////////////////////////////////// // load all sensor data. A/G/M will be handled, and Linear Acc will be saved for comparision. /////////////////////////////////////////////////////////////////////////////////////////////

u=file('open',strFile,'old');

// discard the first line Ah=read(u,1,1,'(a)'); // read all line, and first 7 column. In scilab, index start from 1. MA=[]; MA=read(u,-1,7); fsize=size(MA); SensorInstIndex=csvRead(strFile,[],[],[],[],[],[2,MultiTypeIndex,fsize(1)+1,MultiTypeIndex]); file('close',u); // adjust the time, cut the uesless large part //t_temp=MA(1,3)-pmodulo(MA(1,3),1e6); //MA(:,3)=MA(:,3)-t_temp; //t_temp=MA(1,2)-pmodulo(MA(1,2),1e6); //MA(:,2)=MA(:,2)-t_temp;

[SRow,SCol]=size(MA); AccNumRow=0;GyrNumRow=0;MagNumRow=0;LAccNumRow=0; fbIn=[];wbIn=[];magIn=[];srcLacc=[]; // ouput array lacc=[];wb=[]; Kq=[];// output q array Kb=[];// output bias array KP=[];// P matrix, 6x6 KK=[];// K matrix, 6x6 adx=[];// delta X mdx=[]; Xi=0;// X=[Kq,Kb]', Xi is the index gyroVar=1e-7;biasVar=1e-8;// from data sheet DCM=eye(3,3);// will be updated. From global to local Grav=9.8;// will be updated in init ////// semi constant ///////////////////////////////////

// read data one by one. Note: the calculation q and DCM, are all based on // Global to Local. That is V(l)=C*V(g) // and q is q4 standalone. bInited=0; tMn=[];tMup=[];tgDCM=[];taDCM=[];tmDCM=[]; tCn=[];tCup=[];mdelta=[];adelta=[];amdelta=[]; tgKb=[];taKb=[];tmKb=[]; bAccMeasReady=%f;bMagMeasReady=%f;bUpdateXi=0;aUpdateXi=0;mUpdateXi=0; AccThresholdCounter=0; MapLACC=[]; fori=1:SRow-1 /////////////////////////////////////////////////// // init if needed

if(bInited<InitNum*3)then// 50 point for each data // check sensor type ifMA(i,typeIndex)==TYPEACC&SensorInstIndex(i)==SUBTYPEACCthen // save data AccNumRow=AccNumRow+1; fbIn(AccNumRow,:)=MA(i,dataPos:dataPos+2); if(AccNumRow<=InitNum)thenbInited=bInited+1;end; elseifMA(i,typeIndex)==TYPEMAG&SensorInstIndex(i)==SUBTYPEMAGthen MagNumRow=MagNumRow+1; magIn(MagNumRow,:)=MA(i,dataPos:dataPos+2); if(MagNumRow<=InitNum)thenbInited=bInited+1;end; elseifMA(i,typeIndex)==TYPEGYRO&SensorInstIndex(i)==SUBTYPEGYROthen GyrNumRow=GyrNumRow+1; wbIn(GyrNumRow,:)=MA(i,dataPos:dataPos+2); if(GyrNumRow<=InitNum)thenbInited=bInited+1;end; elseifMA(i,typeIndex)==TYPELACC&SensorInstIndex(i)==SUBTYPELACCthen LAccNumRow=LAccNumRow+1; srcLacc(LAccNumRow,:)=MA(i,dataPos:dataPos+2); end end // continue to check init or not ifInitNum*3==bInitedthen// all init value ready // calc init p. Average A/G for p. AvgG=mean(fbIn(1:InitNum,:),'r'); AvgM=mean(magIn(1:InitNum,:),'r'); Mup=AvgG/norm(AvgG);Grav=norm(AvgG);// init gravity Mmg=AvgM/norm(AvgM); Me=CrossProduct(Mmg,Mup)'; Me=Me/norm(Me); Mn=(CrossProduct(Mup,Me))'; Mn=Mn/norm(Mn); DCM=[Me',Mn',Mup'];// global to body if(norm(DCM)>1.5)thenpause;end; tv=dcmbn2qua(DCM');// The input Rotation matrix is body to global Xi=Xi+1; Kq(Xi,:)=[tv(2:4)',tv(1)]; // calc init gyro noise and bias Kb(Xi,:)=mean(wbIn(1:InitNum,:),'r'); bInited=bInited+1;// finish init signal // init P matrix KP(Xi,:,:)=zeros(6,6);// let's think no error at the start point. Not good suggestion. // init GQGT GQGT=zeros(6,6); GQGT(1:3,1:3)=gyroVar*DT*eye(3,3);

GQGT(4:6,4:6)=biasVar*DT*eye(3,3); GQGT(4:6,1:3)=-0.5*biasVar*DT*DT*eye(3,3); GQGT(1:3,4:6)=GQGT(4:6,1:3); // init delta X dx(Xi,:)=zeros(1,6); // init new gyro data wb(Xi,:)=wbIn(GyrNumRow,:)-Kb(Xi,:);// w(k/k) lacc(Xi,:)=[0,0,0]; // init input index AccNumRow=1;MagNumRow=1;GyrNumRow=0; fbIn(AccNumRow,:)=AvgG;magIn(MagNumRow,:)=AvgM; AccThresholdCounter=aUpdateThred; continue;// init finished elseifInitNum*3>bInitedthen continue;// not inited, continue to read data end

//

pause;

///////////////////////////////////////////////////////// // init finished. Start to calc the lacc // check sensor typeSome strange in the if condition. This is used to control the update algorithm selection. // if (MA(i,typeIndex)==TYPEACC & SensorInstIndex(i)==SUBTYPEACC) | (MA(i,typeIndex)==TYPEMAG &

SensorInstIndex(i)==SUBTYPEMAG) then if(MA(i,typeIndex)==TYPEACC&SensorInstIndex(i)==SUBTYPEACC)&(MA(i,typeIndex)==TYPEMAG&SensorInstIndex(i)==SUB TYPEMAG)then if(MA(i,typeIndex)==TYPEACC&SensorInstIndex(i)==SUBTYPEACC)then bAccMeasReady=%t; AccNumRow=AccNumRow+1; fbIn(AccNumRow,:)=MA(i,dataPos:dataPos+2); end if(MA(i,typeIndex)==TYPEMAG&SensorInstIndex(i)==SUBTYPEMAG)then bMagMeasReady=%t; MagNumRow=MagNumRow+1; magIn(MagNumRow,:)=MA(i,dataPos:dataPos+2); end // update ifbAccMeasReady&bMagMeasReadythen if58<=Xithen // // end if(bUpdateXi==Xi)then mprintf("already updated."); continue;// no update for already updated Xi. mprintf("start of update"); pause;

end bUpdateXi=Xi; bMagMeasReady=%f;bAccMeasReady=%f; KH=zeros(6,6); Sup=[0,0,1];Sn=[0,1,0]; Cup=DCM*(Sup');// will compare with [0,0,1] Cn=DCM*(Sn');// will compare with [0,1,0] KH(1:3,1:3)=VectorCross(Cup); KH(4:6,1:3)=VectorCross(Cn); // assume R=0 t_tv=[]; t_tv=matrix(KP(Xi,:,:),6,6); KK(Xi,:,:)=t_tv*(KH')/(KH*t_tv*(KH')); // dx calc Mup=fbIn(AccNumRow,:)/norm(fbIn(AccNumRow,:)); Mmg=magIn(MagNumRow,:)/norm(magIn(MagNumRow,:)); Mn=(CrossProduct(Mup,CrossProduct(Mmg,Mup)))'; Mn=Mn/norm(Mn); delta=zeros(1,6); delta(1:3)=Mup-Cup'; delta(4:6)=Mn-Cn'; amdelta(Xi,:)=delta; amdx(Xi,:)=(matrix(KK(Xi,:,:),6,6)*delta')'; // update X and P. Calc dq t_tv=[];t_tv=amdx(Xi,1:3)/2.0; ift_tv*t_tv'<1then dq=[t_tv,sqrt(1-t_tv*t_tv')]; else dq=[t_tv,1]/sqrt(1+t_tv*t_tv'); end // // t_tv=[];t_tv=QuaternionProduct([dq(4),dq(1:3)],[Kq(Xi,4),Kq(Xi,1:3)]); Kq(Xi,:)=[t_tv(2:4)',t_tv(1)];

Kq(Xi,:)=QuaternionProduct(dq,Kq(Xi,:))'; Kb(Xi,:)=Kb(Xi,:)+amdx(Xi,4:6); wb(Xi,:)=wb(Xi,:)-amdx(Xi,4:6); // IwbUpdated(i+1,:)=wbIn(i+1,2:4)-IKb(i+1,:);// w(k+1/k+1)

t_tv=[];t_tv=matrix(KK(Xi,:,:),6,6); KP(Xi,:,:)=(eye(6,6)-t_tv*KH)*matrix(KP(Xi,:,:),6,6)*((eye(6,6)-t_tv*KH)'); if58<=Xithen // // end end elseifMA(i,typeIndex)==TYPEACC&SensorInstIndex(i)==SUBTYPEACCthen mprintf("stop of update"); pause;

// save data AccNumRow=AccNumRow+1; fbIn(AccNumRow,:)=MA(i,dataPos:dataPos+2); // pause;

// must 300 ms continuous near 9.8, then the value will be used for update. if(abs(9.8-norm(fbIn(AccNumRow,:)))/9.8<=aUpdateVar)then AccThresholdCounter=AccThresholdCounter+1; if(AccThresholdCounter>aUpdateThred)thenAccThresholdCounter=aUpdateThred;end; else AccThresholdCounter=0; end ifAccThresholdCounter<aUpdateThredthen continue;// update threshold not statisfied. end if(aUpdateXi==Xi)thencontinue;end;// already updated for acc aUpdateXi=Xi;

// update the current X KH=zeros(3,6); Sup=[0,0,1]; Cup=DCM*(Sup'); tCup(Xi,:)=Cup'; KH(1:3,1:3)=VectorCross(Cup); // assume R=0 t_tv=matrix(KP(Xi,:,:),6,6); KK=t_tv*(KH')/(KH*t_tv*(KH')); Mup=fbIn(AccNumRow,:)/norm(fbIn(AccNumRow,:)); tMup(Xi,:)=Mup; delta=Mup-Cup'; adelta(Xi,:)=delta; adx(Xi,:)=(KK*delta')'; t_tv=(adx(Xi,1:3)/2.0); ift_tv*t_tv'<1then dq=[t_tv,sqrt(1-t_tv*t_tv')]; else dq=[t_tv,1]/sqrt(1+t_tv*t_tv'); end // // t_tv=QuaternionProduct([dq(4),dq(1:3)],[Kq(Xi,4),Kq(Xi,1:3)])'; Kq(Xi,:)=[t_tv(2:4),t_tv(1)];

Kq(Xi,:)=QuaternionProduct(dq,Kq(Xi,:))'; Kb(Xi,:)=Kb(Xi,:)+adx(Xi,4:6); taKb(Xi,:)=Kb(Xi,:); wb(Xi,:)=wb(Xi,:)-adx(Xi,4:6); KP(Xi,:,:)=(eye(6,6)-KK*KH)*matrix(KP(Xi,:,:),6,6)*((eye(6,6)-KK*KH)');

// update DCM with new Kq. Global to body DCM=(qua2dcm([Kq(Xi,4),Kq(Xi,1:3)]))'; taDCM(Xi,:,:)=DCM; if(norm(DCM)>1.5)thenpause;end; // pause;

elseifMA(i,typeIndex)==TYPEMAG&SensorInstIndex(i)==SUBTYPEMAGthen MagNumRow=MagNumRow+1; magIn(MagNumRow,:)=MA(i,dataPos:dataPos+2); if(norm(magIn(MagNumRow,:))>MaxMag)thencontinue;end;// something wrong with magnitude ifmUpdateXi==Xithencontinue;end;// already updated for mag mUpdateXi=Xi; // update the current X KH=zeros(3,6); Sn=[0,1,0]; Cn=DCM*(Sn'); tCn(Xi,:)=Cn'; KH(1:3,1:3)=VectorCross(Cn); // assume R=0 t_tv=matrix(KP(Xi,:,:),6,6); KK=t_tv*(KH')/(KH*t_tv*(KH')); Mmg=magIn(MagNumRow,:)/norm(magIn(MagNumRow,:)); Mn=(CrossProduct(Mup,CrossProduct(Mmg,Mup)))';// just use the Mup Mn=Mn/norm(Mn); tMn(Xi,:)=Mn; delta=Mn-Cn'; mdelta(Xi,:)=delta; mdx(Xi,:)=(KK*delta')'; t_tv=(mdx(Xi,1:3)/2.0); ift_tv*t_tv'<1then dq=[t_tv,sqrt(1-t_tv*t_tv')]; else dq=[t_tv,1]/sqrt(1+t_tv*t_tv'); end // // t_tv=QuaternionProduct([dq(4),dq(1:3)],[Kq(Xi,4),Kq(Xi,1:3)])'; Kq(Xi,:)=[t_tv(2:4),t_tv(1)];

Kq(Xi,:)=QuaternionProduct(dq,Kq(Xi,:))'; Kb(Xi,:)=Kb(Xi,:)+mdx(Xi,4:6); tmKb(Xi,:)=Kb(Xi,:); wb(Xi,:)=wb(Xi,:)-mdx(Xi,4:6); KP(Xi,:,:)=(eye(6,6)-KK*KH)*matrix(KP(Xi,:,:),6,6)*((eye(6,6)-KK*KH)'); // update DCM with new Kq DCM=(qua2dcm([Kq(Xi,4),Kq(Xi,1:3)]))'; tmDCM(Xi,:,:)=DCM; if(norm(DCM)>1.5)thenpause;end;

//

pause;

elseifMA(i,typeIndex)==TYPEGYRO&SensorInstIndex(i)==SUBTYPEGYROthen GyrNumRow=GyrNumRow+1; wbIn(GyrNumRow,:)=MA(i,dataPos:dataPos+2);//+[0.03,-0.03,0.06]; // predict rotation matrix Kb(Xi+1,:)=Kb(Xi,:);// b(k+1/k); tgKb(Xi+1,:)=Kb(Xi+1,:); wb(Xi+1,:)=wbIn(GyrNumRow,:)-Kb(Xi+1,:);// w(k+1/k) // q propagation equation. First order quaternion integrator t_wb=(wb(Xi+1,:)+wb(Xi,:))/2.0;// (w(k+1/k)+w(k/k))/2 Dsita=t_wb*DT; DsitaL=norm(Dsita); MDSita=cos(DsitaL/2)*eye(4,4)+sin(DsitaL/2)*Omiga(Dsita)/DsitaL; wbk1=wb(Xi+1,:);wbk=wb(Xi,:); MDSita=MDSita+(Omiga(wbk1)*Omiga(wbk)-Omiga(wbk)*Omiga(wbk1))*DT*DT/48.0; Kq(Xi+1,:)=(MDSita*(Kq(Xi,:)'))';// based on q4 is different, and q(k+1/k) // P prediction t_wb=wb(Xi,:);// use w(k/k) wbl=norm(t_wb); Psi=eye(6,6); t_tv=(1-cos(wbl*DT))/(wbl*wbl); Psi(1:3,1:3)=eye(3,3)-sin(wbl*DT)*VectorCross(t_wb)/wbl+t_tv*VectorCross(t_wb)*VectorCross(t_wb); Psi(1:3,4:6)=-eye(3,3)*DT+t_tv*VectorCross(t_wb)-(wbl*DT-sin(wbl*DT))*VectorCross(t_wb)*VectorCross(t_wb)/(wbl^3); KP(Xi+1,:,:)=Psi*matrix(KP(Xi,:,:),6,6)*(Psi')+GQGT; // lacc prdict. Calc Global to body cos matrix first DCM=(qua2dcm([Kq(Xi+1,4),Kq(Xi+1,1:3)]))'; tgDCM(Xi+1,:,:)=DCM; if(norm(DCM)>1.5)thenpause;end; lacc(Xi+1,:)=fbIn(AccNumRow,:)-(DCM*[0,0,Grav]')';// use the latest acc to calc the linear acc. MapLACC(Xi+1)=AccNumRow; // next step Xi=Xi+1; if58<=Xithen // // end // pause; mprintf("end of predict"); pause;

elseifMA(i,typeIndex)==TYPELACC&SensorInstIndex(i)==SUBTYPELACCthen LAccNumRow=LAccNumRow+1; srcLacc(LAccNumRow,:)=MA(i,dataPos:dataPos+2); end end


相关文章:
卡尔曼滤波那点事---Android 9DOF算法
卡尔曼滤波那点事---Android 9DOF算法_信息与通信_工程科技_专业资料。从基本原理,到android9DOF算法实现。包括原理和scilab(类matlab)的实现代码。卡尔...
卡尔曼滤波算法总结
Q_angle=0.001; Q_gyro=0.003; 以及系统的初始量 angle 还有 Q_bias 还有预测误差矩阵 P,程序里给的是 0(数组) 理论上由于卡尔曼滤波是迭代的算法,当时间...
经典的卡尔曼滤波算法
经典的卡尔曼滤波算法_机械/仪表_工程科技_专业资料。卡尔曼滤波算法自适应卡尔曼滤波卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的, 随着滤波的推进,卡尔曼滤波估计...
卡尔曼滤波算法
卡尔曼滤波算法_电子/电路_工程科技_专业资料。卡尔曼滤波算法 卡尔曼滤波器是一个“最优化自回归数据处理算法” 。对于解决大部分的问题,它是最优, 效率最高...
几种卡尔曼滤波算法理论
几种卡尔曼滤波算法理论_信息与通信_工程科技_专业资料。自适应卡尔曼滤波、扩展卡尔曼滤波和无味卡尔曼滤波简介 自适应卡尔曼滤波卡尔曼滤波发散的原因如果卡尔曼...
卡尔曼滤波算法代码总结(20150311)
*/ angle += K_0 * angle_err; q_bias += K_1 * angle_err; } http://www.docin.com/p-760946791.html //现在智能小车上用的卡尔曼滤波算法。 ...
一个应用实例详解卡尔曼滤波及其算法实现
一个应用实例详解卡尔曼滤波及其算法实现_信息与通信_工程科技_专业资料。一个应用实例详解卡尔曼滤波及其算法实现分类: 数据结构及其算法 为了可以更加容易的理解卡尔...
卡尔曼滤波算法的程序实现和推导过程
卡尔曼滤波算法的程序实现和推导过程 ---蒋海林(QQ:280586940)--卡尔曼滤波算法由匈牙利裔美国数学家鲁道夫·卡尔曼(Rudolf Emil Kalman)创立,这个数学家特么牛 逼...
卡尔曼滤波算法及MATLAB实现
卡尔曼滤波算法及 MATLAB 实现这一段时间对现代滤波进行了学习,对自适应滤波器和卡尔曼滤波器有了一定认识,并对它们用 MATLAB 对语音信号进行了滤 波,发现卡尔曼...
卡尔曼滤波算法C语言实现
卡尔曼滤波算法C语言实现_信息与通信_工程科技_专业资料。详细介绍卡尔曼滤波算法,以及用C语言编程实现卡尔曼滤波算法卡尔曼滤波算法卡尔曼滤波算法及 C 语言...
更多相关标签:
卡尔曼滤波算法 | 扩展卡尔曼滤波算法 | 陀螺仪卡尔曼滤波算法 | 集合卡尔曼滤波算法 | 卡尔曼滤波跟踪算法 | 卡尔曼滤波算法 推导 | 卡尔曼滤波算法 c | 自适应卡尔曼滤波算法 |