PX4飞控中利用EKF估计姿态角代码详解
2015-05-06 23:06
330 查看
PX4飞控中利用EKF估计姿态角代码详解
PX4飞控中主要用EKF算法来估计飞行器三轴姿态角,具体c文件在px4\Firmware\src\modules\attitude_estimator_ekf\codegen\目录下具体原理
程序详解
下一步
1.具体原理
EKF算法原理不再多讲,具体可参见上一篇blog /article/8997253.html.这篇讲EKF算法执行过程,需要以下几个关键式子:
飞行器状态矩阵:
这里
表示三轴角速度,
表示三轴角加速度,
表示加速度在机体坐标系三轴分量,
,表示磁力计在机体坐标系三轴分量。
测量矩阵
分别由三轴陀螺仪,加速度计,磁力计测得。
状态转移矩阵:
飞行器下一时刻状态预测矩阵如下:
其中W项均为高斯噪声,
为飞行器在姿态发生变化后,坐标系余旋变换矩阵,对该函数在
处求一阶偏导,可得到状态转移矩阵:
此时可得到飞行器状态的先验估计:
利用测量值修正先验估计:
这里测量矩阵H与状态矩阵X为线性关系,故无需求偏导。
卡尔曼增益:
状态后验估计:
方差后验估计:
2.程序详解
整个EKF的代码挺长的,大部分是矩阵运算,而且使用嵌套for循环来执行的,所以读起来比较费劲,但是要是移植到自己工程上的话必然离不开这一步,所以花了一个下午把各个细节理清楚,顺便记录分享。/* Include files */ #include "rt_nonfinite.h" #include "attitudeKalmanfilter.h" #include "rdivide.h" #include "norm.h" #include "cross.h" #include "eye.h" #include "mrdivide.h" /* '输入参数:updateVect[3]:用来记录陀螺仪,加速度计,磁力计传感器数值是否有效 z[9] :测量矩阵 x_aposteriori_k[12]:上一时刻状态后验估计矩阵,用来预测当前状态 P_aposteriori_k[144]:上一时刻后验估计方差 eulerAngles[3] :输出欧拉角 Rot_matrix[9] :输出余弦矩阵 x_aposteriori[12] :输出状态后验估计矩阵 P_aposteriori[144] :输出方差后验估计矩阵' */ void attitudeKalmanfilter( const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]) { /*以下这一堆变量用到的时候再解释*/ real32_T wak[3]; real32_T O[9]; real_T dv0[9]; real32_T a[9]; int32_T i; real32_T b_a[9]; real32_T x_n_b[3]; real32_T b_x_aposteriori_k[3]; real32_T z_n_b[3]; real32_T c_a[3]; real32_T d_a[3]; int32_T i0; real32_T x_apriori[12]; real_T dv1[144]; real32_T A_lin[144]; static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T b_A_lin[144]; real32_T b_q[144]; real32_T c_A_lin[144]; real32_T d_A_lin[144]; real32_T e_A_lin[144]; int32_T i1; real32_T P_apriori[144]; real32_T b_P_apriori[108]; static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T K_k[108]; real32_T fv0[81]; static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T b_r[81]; real32_T fv1[81]; real32_T f0; real32_T c_P_apriori[36]= { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T fv2[36]; static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T c_r[9]; real32_T b_K_k[36]; real32_T d_P_apriori[72]; static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }; real32_T c_K_k[72]; static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T b_z[6]; static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T fv3[6]; real32_T c_z[6]; /*开始计算*/ /*'wak[]为当前状态三轴角加速度'*/ wak[0] = x_aposteriori_k[3]; wak[1] = x_aposteriori_k[4]; wak[2] = x_aposteriori_k[5];
/* ‘欧拉角旋转矩阵’
O=⎡⎣⎢0wzwy−wz0wxwy−wx0⎤⎦⎥ O=\left[
\begin{array}{ccc}
0&-w_z&w_y\\
w_z&0&-w_x\\
w_y&w_x&0\\
\end{array}
\right]
这里的O矩阵相当于A矩阵中的
的转置矩阵!
*/
O[0] = 0.0F; O[1] = -x_aposteriori_k[2]; O[2] = x_aposteriori_k[1]; O[3] = x_aposteriori_k[2]; O[4] = 0.0F; O[5] = -x_aposteriori_k[0]; O[6] = -x_aposteriori_k[1]; O[7] = x_aposteriori_k[0]; O[8] = 0.0F; /* 预测转过一个小角度之后的重力向量三轴投影 */ /* a = [1, -delta_z, delta_y; * delta_z, 1 , -delta_x; * -delta_y, delta_x, 1 ]'; */ eye(dv0); //dv0矩阵单位化 for (i = 0; i < 9; i++) { a[i] = (real32_T)dv0[i] + O[i] * dt; } /* 预测转过一个小角度之后的磁力向量三轴投影 */ eye(dv0); for (i = 0; i < 9; i++) { b_a[i] = (real32_T)dv0[i] + O[i] * dt; }
/*
a=⎡⎣⎢1Δz−Δy−Δz1ΔxΔy−Δx1⎤⎦⎥ a=\left[
\begin{array}{ccc}
1&-\Delta z&\Delta y\\
\Delta z&1&-\Delta x\\
-\Delta y&\Delta x&1\\
\end{array}
\right]
其实就是这个大家都很眼熟的的余弦矩阵的转置, 用来更新机体转过一个角度之后的重力和磁力三轴投影,只不过两次计算间隔时间很短,变化角度很小,因此忽略高阶小量之后就变成了这个样子。这里还少一个时间系数dt,下面会补上。
⎡⎣⎢cosy∗cosz−cosy∗sinzsiny(sinx∗siny∗cosz)+(cosx∗sinz)−(sinx∗siny∗sinz)+(cosx∗cosz)−sinx∗cosy−(cosx∗siny∗cosz)+(sinx∗sinz)(cosx∗siny∗sinz)+(sinx∗cosz)cosx∗cosy⎤⎦⎥ \left[
\begin{array}{ccc}
cosy * cosz&(sinx * siny * cosz) + (cosx * sinz)&-(cosx * siny * cosz) + (sinx * sinz)\\
-cosy * sinz&-(sinx * siny * sinz) + (cosx * cosz)&(cosx * siny * sinz) + (sinx * cosz)\\
siny&-sinx * cosy&cosx * cosy
\end{array}
\right]
*/
x_n_b[0] = x_aposteriori_k[0]; //角速度 x_n_b[1] = x_aposteriori_k[1]; x_n_b[2] = x_aposteriori_k[2]; b_x_aposteriori_k[0] = x_aposteriori_k[6]; // 加速度 b_x_aposteriori_k[1] = x_aposteriori_k[7]; b_x_aposteriori_k[2] = x_aposteriori_k[8]; z_n_b[0] = x_aposteriori_k[9]; //磁力计 z_n_b[1] = x_aposteriori_k[10]; z_n_b[2] = x_aposteriori_k[11]; for (i = 0; i < 3; i++) { c_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; } d_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; } x_apriori[i] = x_n_b[i] + dt * wak[i]; } for (i = 0; i < 3; i++) { x_apriori[i + 3] = wak[i]; } for (i = 0; i < 3; i++) { x_apriori[i + 6] = c_a[i]; } for (i = 0; i < 3; i++) { x_apriori[i + 9] = d_a[i]; } //得到状态先验估计
/*
根据上述矩阵运算,可以得到:
c_a[1∗3]=[axayaz]∗a[3∗3] c\_a[1*3]=\left[
\begin{array}{ccc}
a_x&a_y&a_z
\end{array}
\right] * a[3*3 ]
从而:ω˜kra,kΔt[3∗1]=c_a[1∗3]T\widetilde\omega _kr_a,_k\Delta t[3*1] = c\_a[1*3]^T
d_a[1∗3]=[mxmymz]∗a[3∗3] d\_a[1*3]=\left[
\begin{array}{ccc}
m_x&m_y&m_z
\end{array}
\right] * a[3*3 ]
从而:ω˜krm,kΔt[3∗1]=d_a[1∗3]T\widetilde\omega _kr_m,_k\Delta t[3*1] = d\_a[1*3]^T
其中[axayaz]和[mxmymz]分别对应ra,k和rm,k的转置; \left[
\begin{array}{ccc}
a_x&a_y&a_z
\end{array}
\right]和 \left[
\begin{array}{ccc}
m_x&m_y&m_z
\end{array}
\right]分别对应r_a,_k和r_m,_k的转置;
得到状态先验估计:
Xk+1[12∗1]=x_apriori[1∗12]T X_k +_1[12*1]=x\_apriori[1*12]^T=[x_n_b+wak∗dtwakc_ad_a]T =\left[
\begin{array}{cccc}
x\_n\_b+wak*dt&wak & c\_a&d\_a\\
\end{array}
\right]^T
*/
/* '开始计算A矩阵'*/ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; } /*1 2 3列*/ for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * i) + 3] = 0.0F; } /*3 4 5列*/ } /*6 7 8 列*/ A_lin[6] = 0.0F; A_lin[7] = x_aposteriori_k[8]; A_lin[8] = -x_aposteriori_k[7]; A_lin[18] = -x_aposteriori_k[8]; A_lin[19] = 0.0F; A_lin[20] = x_aposteriori_k[6]; A_lin[30] = x_aposteriori_k[7]; A_lin[31] = -x_aposteriori_k[6]; A_lin[32] = 0.0F; for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; } } /*6 7 8 列*/ /*9 10 11 列*/ A_lin[9] = 0.0F; A_lin[10] = x_aposteriori_k[11]; A_lin[11] = -x_aposteriori_k[10]; A_lin[21] = -x_aposteriori_k[11]; A_lin[22] = 0.0F; A_lin[23] = x_aposteriori_k[9]; A_lin[33] = x_aposteriori_k[7]; A_lin[34] = -x_aposteriori_k[9]; A_lin[35] = 0.0F; for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; } } /*9 10 11 列*/
/*
根据上述矩阵运算,可以得到A_lin矩阵:
A_lin[12∗12]=⎡⎣⎢⎢⎢0I000000A10O0A200O⎤⎦⎥⎥⎥ A\_lin[12*12]=\left[
\begin{array}{cccc}
0&0&A_1&A_2\\
I&0&0&0\\
0&0&O&0\\
0&0&0&O\\
\end{array}
\right]其中各元素都是3*3的矩阵;I为单位矩阵,其中
A1=⎡⎣⎢0−azayaz0−ax−ayax0⎤⎦⎥=−r˜a,Tk A_1=\left[
\begin{array}{ccc}
0&a_z&-a_y\\
-a_z&0&a_x\\
a_y&-a_x&0\\
\end{array}
\right]= - \widetilde r_a,_k ^T
同样A2=⎡⎣⎢0−mzmymz0−mx−mymx0⎤⎦⎥=−r˜m,Tk A_2=\left[
\begin{array}{ccc}
0&m_z&-m_y\\
-m_z&0&m_x\\
m_y&-m_x&0\\
\end{array}
\right]= - \widetilde r_m,_k ^T
*/
for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *dt; } } //最终A_link,k的逆矩阵
得到:
Alin,Tk=b_A_lin[12∗12]=⎡⎣⎢⎢⎢I0000I0000I0000I⎤⎦⎥⎥⎥+⎡⎣⎢⎢⎢0I000000A10O0A200O⎤⎦⎥⎥⎥∗dt A_lin , _k^T=b\_A\_lin[12*12 ] = \left[
\begin{array}{cccc}
I&0&0&0\\
0&I&0&0\\
0&0&I&0\\
0&0&0&I
\end{array}
\right]+\left[
\begin{array}{cccc}
0&0&A_1&A_2\\
I&0&0&0\\
0&0&O&0\\
0&0&0&O\\
\end{array}
\right]*dt
/*
开始根据
计算过程方差
*/
过程噪声方差b_q[12∗12]=⎡⎣⎢⎢⎢⎢q00000q10000q20000q3⎤⎦⎥⎥⎥⎥ 过程噪声方差 b\_q[12*12]=\left[
\begin{array}{cccc}
q_0&0&0&0\\
0&q_1&0&0\\
0&0&q_2&0\\
0&0&0&q_3\\
\end{array}
\right]其中各元素都是3*3的矩阵;
b_q[0] = q[0]; b_q[12] = 0.0F; b_q[24] = 0.0F; b_q[36] = 0.0F; b_q[48] = 0.0F; b_q[60] = 0.0F; b_q[72] = 0.0F; b_q[84] = 0.0F; b_q[96] = 0.0F; b_q[108] = 0.0F; b_q[120] = 0.0F; b_q[132] = 0.0F; b_q[1] = 0.0F; b_q[13] = q[0]; b_q[25] = 0.0F; b_q[37] = 0.0F; b_q[49] = 0.0F; b_q[61] = 0.0F; b_q[73] = 0.0F; b_q[85] = 0.0F; b_q[97] = 0.0F; b_q[109] = 0.0F; b_q[121] = 0.0F; b_q[133] = 0.0F; b_q[2] = 0.0F; b_q[14] = 0.0F; b_q[26] = q[0]; b_q[38] = 0.0F; b_q[50] = 0.0F; b_q[62] = 0.0F; b_q[74] = 0.0F; b_q[86] = 0.0F; b_q[98] = 0.0F; b_q[110] = 0.0F; b_q[122] = 0.0F; b_q[134] = 0.0F; b_q[3] = 0.0F; b_q[15] = 0.0F; b_q[27] = 0.0F; b_q[39] = q[1]; b_q[51] = 0.0F; b_q[63] = 0.0F; b_q[75] = 0.0F; b_q[87] = 0.0F; b_q[99] = 0.0F; b_q[111] = 0.0F; b_q[123] = 0.0F; b_q[135] = 0.0F; b_q[4] = 0.0F; b_q[16] = 0.0F; b_q[28] = 0.0F; b_q[40] = 0.0F; b_q[52] = q[1]; b_q[64] = 0.0F; b_q[76] = 0.0F; b_q[88] = 0.0F; b_q[100] = 0.0F; b_q[112] = 0.0F; b_q[124] = 0.0F; b_q[136] = 0.0F; b_q[5] = 0.0F; b_q[17] = 0.0F; b_q[29] = 0.0F; b_q[41] = 0.0F; b_q[53] = 0.0F; b_q[65] = q[1]; b_q[77] = 0.0F; b_q[89] = 0.0F; b_q[101] = 0.0F; b_q[113] = 0.0F; b_q[125] = 0.0F; b_q[137] = 0.0F; b_q[6] = 0.0F; b_q[18] = 0.0F; b_q[30] = 0.0F; b_q[42] = 0.0F; b_q[54] = 0.0F; b_q[66] = 0.0F; b_q[78] = q[2]; b_q[90] = 0.0F; b_q[102] = 0.0F; b_q[114] = 0.0F; b_q[126] = 0.0F; b_q[138] = 0.0F; b_q[7] = 0.0F; b_q[19] = 0.0F; b_q[31] = 0.0F; b_q[43] = 0.0F; b_q[55] = 0.0F; b_q[67] = 0.0F; b_q[79] = 0.0F; b_q[91] = q[2]; b_q[103] = 0.0F; b_q[115] = 0.0F; b_q[127] = 0.0F; b_q[139] = 0.0F; b_q[8] = 0.0F; b_q[20] = 0.0F; b_q[32] = 0.0F; b_q[44] = 0.0F; b_q[56] = 0.0F; b_q[68] = 0.0F; b_q[80] = 0.0F; b_q[92] = 0.0F; b_q[104] = q[2]; b_q[116] = 0.0F; b_q[128] = 0.0F; b_q[140] = 0.0F; b_q[9] = 0.0F; b_q[21] = 0.0F; b_q[33] = 0.0F; b_q[45] = 0.0F; b_q[57] = 0.0F; b_q[69] = 0.0F; b_q[81] = 0.0F; b_q[93] = 0.0F; b_q[105] = 0.0F; b_q[117] = q[3]; b_q[129] = 0.0F; b_q[141] = 0.0F; b_q[10] = 0.0F; b_q[22] = 0.0F; b_q[34] = 0.0F; b_q[46] = 0.0F; b_q[58] = 0.0F; b_q[70] = 0.0F; b_q[82] = 0.0F; b_q[94] = 0.0F; b_q[106] = 0.0F; b_q[118] = 0.0F; b_q[130] = q[3]; b_q[142] = 0.0F; b_q[11] = 0.0F; b_q[23] = 0.0F; b_q[35] = 0.0F; b_q[47] = 0.0F; b_q[59] = 0.0F; b_q[71] = 0.0F; b_q[83] = 0.0F; b_q[95] = 0.0F; b_q[107] = 0.0F; b_q[119] = 0.0F; b_q[131] = 0.0F; b_q[143] = q[3]; for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *i0]; } c_A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; } } for (i0 = 0; i0 < 12; i0++) { d_A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } e_A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } } }
根据上面的矩阵运算,可以得到:
A_lin[12∗12]=Pk[12∗12]∗bA_lin[12∗12];A\_lin[12*12] = P_k[12*12]*b_A\_lin[12*12];
d_A_lin[12∗12]=b_A_lin[12∗12]T∗A_lin[12∗12]=Alin,kPkAlin,Tk;
d\_A\_lin[12*12] = b\_A\_lin[12*12]^T*A\_lin[12*12]=A_lin,_kP_kA_lin,_k^T;
c_A_lin[12∗12]=Pq[12∗12]∗bA_lin[12∗12];c\_A\_lin[12*12] = P_q[12*12]*b_A\_lin[12*12];
e_A_lin[12∗12]=b_A_lin[12∗12]T∗c_A_lin[12∗12]=Alin,kQkAlin,Tk;
e\_A\_lin[12*12] = b\_A\_lin[12*12]^T*c\_A\_lin[12*12]=A_lin,_kQ_kA_lin,_k^T;
for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; } } //最终过程方差
最终得到过程方差::
P_apriori[12∗12]=d_A_lin[12∗12]+e_A_lin[12∗12];P\_apriori[12*12] = d\_A\_lin[12*12] + e\_A\_lin[12*12];
/*
下面开始利用测量值修正先验估计:用到的公式为:
*/
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { /*都为1表示三个传感器测量值均有效*/ if ((z[5] < 4.0F) || (z[4] > 15.0F)) { r[1] = 10000.0F; } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 9; i0++) { b_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + 12 * i0]; } } } for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 12; i0++) { K_k[i + 9 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; } } for (i0 = 0; i0 < 9; i0++) { fv0[i + 9 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; } } }
同样是计算了一堆中间矩阵:
b_P_apriori[9∗12]=iv1[9∗12]∗P_apriori[12∗12]=HkPk;b\_P\_apriori[9*12] = iv1[9*12] * P\_apriori[12*12] = H_kP_k;
其中:iv1[9∗12]=⎡⎣⎢I000000I000I⎤⎦⎥=Hk 其中:iv1[9*12]=\left[
\begin{array}{cccc}
I&0&0&0\\
0&0&I&0\\
0&0&0&I\\
\end{array}
\right] = H_kk_k[12∗9]=P_apriori[12∗12]∗iv2[9∗12]=PkHTk;k\_k[12*9] = P\_apriori[12*12] * iv2[9*12] = P_kH_k^T;
iv2[12∗9]=⎡⎣⎢⎢⎢I00000I0000I⎤⎦⎥⎥⎥=HTk iv2[12*9]=\left[
\begin{array}{ccc}
I&0&0\\
0&0&0\\
0&I&0\\
0&0&I\\
\end{array}
\right] = H_k^T
fv0[9∗9]=⎡⎣⎢I00000010001⎤⎦⎥∗k_k[12∗9]=HkPkHTk fv0[9*9]=\left[
\begin{array}{cccc}
I&0&0&0\\
0&0&1&0\\
0&0&0&1\\
\end{array}
\right] * k\_k[12*9] = H_kP_kH_k^T
测量噪声方差b_r[9∗9]=⎡⎣⎢r0000r1000r2⎤⎦⎥其中各元素都是3∗3的矩阵; 测量噪声方差 b\_r[9*9]=\left[
\begin{array}{ccc}
r_0&0&0\\
0&r_1&0\\
0&0&r_2\\
\end{array}
\right]其中各元素都是3*3的矩阵;
b_r[0] = r[0]; b_r[9] = 0.0F; b_r[18] = 0.0F; b_r[27] = 0.0F; b_r[36] = 0.0F; b_r[45] = 0.0F; b_r[54] = 0.0F; b_r[63] = 0.0F; b_r[72] = 0.0F; b_r[1] = 0.0F; b_r[10] = r[0]; b_r[19] = 0.0F; b_r[28] = 0.0F; b_r[37] = 0.0F; b_r[46] = 0.0F; b_r[55] = 0.0F; b_r[64] = 0.0F; b_r[73] = 0.0F; b_r[2] = 0.0F; b_r[11] = 0.0F; b_r[20] = r[0]; b_r[29] = 0.0F; b_r[38] = 0.0F; b_r[47] = 0.0F; b_r[56] = 0.0F; b_r[65] = 0.0F; b_r[74] = 0.0F; b_r[3] = 0.0F; b_r[12] = 0.0F; b_r[21] = 0.0F; b_r[30] = r[1]; b_r[39] = 0.0F; b_r[48] = 0.0F; b_r[57] = 0.0F; b_r[66] = 0.0F; b_r[75] = 0.0F; b_r[4] = 0.0F; b_r[13] = 0.0F; b_r[22] = 0.0F; b_r[31] = 0.0F; b_r[40] = r[1]; b_r[49] = 0.0F; b_r[58] = 0.0F; b_r[67] = 0.0F; b_r[76] = 0.0F; b_r[5] = 0.0F; b_r[14] = 0.0F; b_r[23] = 0.0F; b_r[32] = 0.0F; b_r[41] = 0.0F; b_r[50] = r[1]; b_r[59] = 0.0F; b_r[68] = 0.0F; b_r[77] = 0.0F; b_r[6] = 0.0F; b_r[15] = 0.0F; b_r[24] = 0.0F; b_r[33] = 0.0F; b_r[42] = 0.0F; b_r[51] = 0.0F; b_r[60] = r[2]; b_r[69] = 0.0F; b_r[78] = 0.0F; b_r[7] = 0.0F; b_r[16] = 0.0F; b_r[25] = 0.0F; b_r[34] = 0.0F; b_r[43] = 0.0F; b_r[52] = 0.0F; b_r[61] = 0.0F; b_r[70] = r[2]; b_r[79] = 0.0F; b_r[8] = 0.0F; b_r[17] = 0.0F; b_r[26] = 0.0F; b_r[35] = 0.0F; b_r[44] = 0.0F; b_r[53] = 0.0F; b_r[62] = 0.0F; b_r[71] = 0.0F; b_r[80] = r[2]; for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 9; i0++) { fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; } }
fv1[9∗9]=fv0[9∗9]+⎡⎣⎢r0000r1000r2⎤⎦⎥=HkPkHTk+R fv1[9*9]=fv0[9*9] + \left[
\begin{array}{ccc}
r_0&0&0\\
0&r_1&0\\
0&0&r_2\\
\end{array}
\right]=H_kP_kH_k^T+R
/*矩 阵 除 法 ,计算出卡尔曼增益*/ mrdivide(b_P_apriori, fv1, K_k);
这个函数的作用是计算卡尔曼增益:Kk[12∗9]T=K_K[9∗12]=b_P_apriori[9∗12]fv1[9∗9] 这个函数的作用是计算卡尔曼增益:
K_k[12*9]^T=K\_K[9*12]= \frac{b\_P\_apriori[9*12] }{fv1[9*9]}
/* x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 9; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; } O[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 9; i0++) { f0 += K_k[i + 12 * i0] * O[i0]; } x_aposteriori[i] = x_apriori[i] + f0; }
计算状态后验估计:
O[1∗9]=z[1∗9]−x_apriori[1∗12]∗HT[12∗9]
O[1*9]= z[1*9] - x\_apriori[1*12]*H^T[12*9]得到:x^k[12∗1]T=x_aposteriori[1∗12]
\hat x_k[12*1]^T= x\_aposteriori[1*12]=x_apriori[1∗12]+O[1∗9]∗K_K[9∗12]= x\_apriori[1*12] +O[1*9]*K\_K[9*12]
/* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 9; i1++) { f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; } b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } }
计算方差后验估计:
b_A_lin[12∗12]=⎡⎣⎢⎢⎢I0000I0000I0000I⎤⎦⎥⎥⎥−HTk∗K_K b\_A\_lin[12*12 ] = \left[
\begin{array}{cccc}
I&0&0&0\\
0&I&0&0\\
0&0&I&0\\
0&0&0&I
\end{array}
\right]-H_k^T*K\_K得到:Pk[12∗12]T=P_aposteriori[12∗12]
P_k[12*12]^T=P\_aposteriori[12*12]=P_apriori[12∗12]∗b_A_lin[12∗12];= P\_apriori[12*12] *b\_A\_lin[12*12 ];
到此就把所有的量都计算出来了!
下面几种情形为某个传感器未更新的情况,只需改变H矩阵和测量噪声方差矩阵即可,其余运算均类似!
else { /* 'attitudeKalmanfilter:138' else */ /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ /* 'attitudeKalmanfilter:142' 0,r(1),0; */ /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ /* observation matrix */ /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { c_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv3[i1 + 12 * i0]; } } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 12; i0++) { fv2[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * i0]; } } for (i0 = 0; i0 < 3; i0++) { O[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; } } } c_r[0] = r[0]; c_r[3] = 0.0F; c_r[6] = 0.0F; c_r[1] = 0.0F; c_r[4] = r[0]; c_r[7] = 0.0F; c_r[2] = 0.0F; c_r[5] = 0.0F; c_r[8] = r[0]; for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; } } b_mrdivide(c_P_apriori, a, b_K_k); /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; } x_n_b[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 3; i0++) { f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 3; i1++) { f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; } b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:156' else */ /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) { /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ if ((z[5] < 4.0F) || (z[4] > 15.0F)) { /* 'attitudeKalmanfilter:159' r(2)=10000; */ r[1] = 10000.0F; } /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ /* observation matrix */ /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv5[i1 + 12 * i0]; } } } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { c_K_k[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 * i0]; } } for (i0 = 0; i0 < 6; i0++) { fv2[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; } } } b_K_k[0] = r[0]; b_K_k[6] = 0.0F; b_K_k[12] = 0.0F; b_K_k[18] = 0.0F; b_K_k[24] = 0.0F; b_K_k[30] = 0.0F; b_K_k[1] = 0.0F; b_K_k[7] = r[0]; b_K_k[13] = 0.0F; b_K_k[19] = 0.0F; b_K_k[25] = 0.0F; b_K_k[31] = 0.0F; b_K_k[2] = 0.0F; b_K_k[8] = 0.0F; b_K_k[14] = r[0]; b_K_k[20] = 0.0F; b_K_k[26] = 0.0F; b_K_k[32] = 0.0F; b_K_k[3] = 0.0F; b_K_k[9] = 0.0F; b_K_k[15] = 0.0F; b_K_k[21] = r[1]; b_K_k[27] = 0.0F; b_K_k[33] = 0.0F; b_K_k[4] = 0.0F; b_K_k[10] = 0.0F; b_K_k[16] = 0.0F; b_K_k[22] = 0.0F; b_K_k[28] = r[1]; b_K_k[34] = 0.0F; b_K_k[5] = 0.0F; b_K_k[11] = 0.0F; b_K_k[17] = 0.0F; b_K_k[23] = 0.0F; b_K_k[29] = 0.0F; b_K_k[35] = r[1]; for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; } } c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 6; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; } b_z[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { f0 += c_K_k[i + 12 * i0] * b_z[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; } b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:181' else */ /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) { /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ /* observation matrix */ /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv7[i1 + 12 * i0]; } } } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { c_K_k[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + 12 * i0]; } } for (i0 = 0; i0 < 6; i0++) { fv2[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; } } } b_K_k[0] = r[0]; b_K_k[6] = 0.0F; b_K_k[12] = 0.0F; b_K_k[18] = 0.0F; b_K_k[24] = 0.0F; b_K_k[30] = 0.0F; b_K_k[1] = 0.0F; b_K_k[7] = r[0]; b_K_k[13] = 0.0F; b_K_k[19] = 0.0F; b_K_k[25] = 0.0F; b_K_k[31] = 0.0F; b_K_k[2] = 0.0F; b_K_k[8] = 0.0F; b_K_k[14] = r[0]; b_K_k[20] = 0.0F; b_K_k[26] = 0.0F; b_K_k[32] = 0.0F; b_K_k[3] = 0.0F; b_K_k[9] = 0.0F; b_K_k[15] = 0.0F; b_K_k[21] = r[2]; b_K_k[27] = 0.0F; b_K_k[33] = 0.0F; b_K_k[4] = 0.0F; b_K_k[10] = 0.0F; b_K_k[16] = 0.0F; b_K_k[22] = 0.0F; b_K_k[28] = r[2]; b_K_k[34] = 0.0F; b_K_k[5] = 0.0F; b_K_k[11] = 0.0F; b_K_k[17] = 0.0F; b_K_k[23] = 0.0F; b_K_k[29] = 0.0F; b_K_k[35] = r[2]; for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; } } c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { b_z[i] = z[i]; } for (i = 0; i < 3; i++) { b_z[i + 3] = z[i + 6]; } for (i = 0; i < 6; i++) { fv3[i] = 0.0F; for (i0 = 0; i0 < 12; i0++) { fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; } c_z[i] = b_z[i] - fv3[i]; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { f0 += c_K_k[i + 12 * i0] * c_z[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; } b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:202' else */ /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ for (i = 0; i < 12; i++) { x_aposteriori[i] = x_apriori[i]; } /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); } } } }
至此,EKF解算姿态过程全部结束,下面从姿态矩阵中提取欧拉角。其实本质就是计算新的余弦矩阵,然后根据下面的公式计算欧拉角
Rot_matrix=⎡⎣⎢r0r3r6r1r4r7r2r5r8⎤⎦⎥Rot\_matrix= \left[
\begin{array}{ccc}
r_0&r_1&r_2\\
r_3&r_4&r_5\\
r_6&r_7&r_8
\end{array}
\right]ϕ=arctan[r7r8]\phi = arctan[\frac{r_7}{r_8}]
θ=arcsin[−r6]\theta= arcsin[-r_6]
ψ=arcsin[r3r0]\psi= arcsin[\frac{r_3}{r_0}]
/* % euler anglels extraction */ /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ for (i = 0; i < 3; i++) { x_n_b[i] = -x_aposteriori[i + 6]; } rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& x_aposteriori[9]), wak); /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ for (i = 0; i < 3; i++) { x_n_b[i] = wak[i]; } cross(z_n_b, x_n_b, wak); /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ for (i = 0; i < 3; i++) { x_n_b[i] = wak[i]; } rdivide(x_n_b, norm(wak), wak); /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ cross(wak, z_n_b, x_n_b); /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ for (i = 0; i < 3; i++) { b_x_aposteriori_k[i] = x_n_b[i]; } rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ for (i = 0; i < 3; i++) { Rot_matrix[i] = x_n_b[i]; Rot_matrix[3 + i] = wak[i]; Rot_matrix[6 + i] = z_n_b[i]; } /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } /* End of code generation (attitudeKalmanfilter.c) */
3.下一步
把EKF估计姿态原理和具体算法细节搞清楚之后就可以移植到自己的工程上了,完成后把代码放上来。相关文章推荐
- PX4飞控中利用EKF估计姿态角代码详解
- PIXHAWK 飞控中的EKF姿态估计的欧拉角求解
- 利用types增强vscode中js代码提示功能详解
- 第九章 PX4-pixhawk-姿态估计解析
- 实录 | 旷视研究院详解COCO2017人体姿态估计冠军论文(PPT+视频)
- 利用google提供的API(JavaScript接口)获取网站访问者IP地理位置的代码详解
- 利用JNI技术在Android中调用C、调试C++代码(工具,详解)
- 利用Python代码实现数据可视化的5种方法详解
- 第九章 PX4-pixhawk-姿态估计解析
- 基于四元数的扩展卡尔曼滤波器(EKF)姿态估计文献小结
- ppz飞控代码不同姿态表示之间转换理解
- 基于四元数的扩展卡尔曼(EKF)滤波器四旋翼姿态解算详解-1.KF介绍
- golang利用不到20行代码实现路由调度详解
- px4飞控位置估计lpe移植到vs
- 利用vscode调试编译后的js代码详解
- 利用二维图像进行头部姿态估计
- 利用div+css实现几种经典布局的详解,样式以及代码!
- Spring中利用配置文件和@value注入属性值代码详解
- 利用google提供的API(JavaScript接口)获取网站访问者IP地理位置的代码详解
- 基于四元数的扩展卡尔曼(EKF)滤波器四旋翼姿态解算详解-2.EKF介绍