PX4飞控中利用EKF估计姿态角代码详解

PX4飞控中利用EKF估计姿态角代码详解

PX4飞控中主要用EKF算法来估计飞行器三轴姿态角，具体c文件在px4\Firmware\src\modules\attitude_estimator_ekf\codegen\目录下

• 具体原理
• 程序详解
• 下一步

---

1.具体原理

EKF算法原理不再多讲，具体可参见上一篇blog http://blog.csdn.net/lizilpl/article/details/45289541.

这篇讲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];

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69
• 70
• 71
• 72
• 73
• 74
• 75
• 76
• 77
• 78
• 79
• 80
• 81
• 82
• 83
• 84
• 85
• 86
• 87
• 88
• 89
• 90
• 91
• 92
• 93
• 94
• 95
• 96
• 97
• 98
• 99
• 100
• 101
• 102
• 103
• 104
• 105
• 106
• 107
• 108
• 109
• 110
• 111
• 112
• 113
• 114
• 115
• 116
• 117
• 118
• 119
• 120
• 121
• 122
• 123
• 124
• 125
• 126
• 127
• 128
• 129
• 130
• 131
• 132
• 133
• 134
• 135
• 136
• 137
• 138
• 139
• 140
• 141
• 142
• 143
• 144
• 145
• 146
• 147
• 148
• 149
• 150
• 151
• 152
• 153
• 154
• 155
• 156
• 157
• 158
• 159
• 160
• 161
• 162

/* ‘欧拉角旋转矩阵’ 

O=⎡⎣⎢0wzwy−wz0wxwy−wx0⎤⎦⎥O=[0−wzwywz0−wxwywx0]

这里的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;
  }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28

/*

a=⎡⎣⎢1Δz−Δy−Δz1ΔxΔy−Δx1⎤⎦⎥a=[1−ΔzΔyΔz1−Δx−ΔyΔx1]

其实就是这个大家都很眼熟的的余弦矩阵的转置, 用来更新机体转过一个角度之后的重力和磁力三轴投影，只不过两次计算间隔时间很短，变化角度很小，因此忽略高阶小量之后就变成了这个样子。这里还少一个时间系数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⎤⎦⎥[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∗cosycosx∗cosy]

*/

  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];
  }   //得到状态先验估计  

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34

/* 
根据上述矩阵运算，可以得到： 

c_a[1∗3]=[axayaz]∗a[3∗3]c_a[1∗3]=[axayaz]∗a[3∗3]

从而：

ω˜kra,kΔt[3∗1]=c_a[1∗3]Tω~kra,kΔt[3∗1]=c_a[1∗3]T

d_a[1∗3]=[mxmymz]∗a[3∗3]d_a[1∗3]=[mxmymz]∗a[3∗3]

从而：

ω˜krm,kΔt[3∗1]=d_a[1∗3]Tω~krm,kΔt[3∗1]=d_a[1∗3]T

其中

[axayaz]和[mxmymz]分别对应ra,k和rm,k的转置；[axayaz]和[mxmymz]分别对应ra,k和rm,k的转置；

得到状态先验估计: 

Xk+1[12∗1]=x_apriori[1∗12]TXk+1[12∗1]=x_apriori[1∗12]T

=[x_n_b+wak∗dtwakc_ad_a]T=[x_n_b+wak∗dtwakc_ad_a]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 列*/

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68

/* 
根据上述矩阵运算，可以得到A_lin矩阵： 

A_lin[12∗12]=⎡⎣⎢⎢⎢0I000000A10O0A200O⎤⎦⎥⎥⎥A_lin[12∗12]=[00A1A2I00000O0000O]

其中各元素都是3*3的矩阵；I为单位矩阵，其中 

A1=⎡⎣⎢0−azayaz0−ax−ayax0⎤⎦⎥=−r˜a,TkA1=[0az−ay−az0axay−ax0]=−r~a,kT

同样

A2=⎡⎣⎢0−mzmymz0−mx−mymx0⎤⎦⎥=−r˜m,TkA2=[0mz−my−mz0mxmy−mx0]=−r~m,kT

*/

  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的逆矩阵

• 1
• 2
• 3
• 4
• 5

得到： 

Alin,Tk=b_A_lin[12∗12]=⎡⎣⎢⎢⎢I0000I0000I0000I⎤⎦⎥⎥⎥+⎡⎣⎢⎢⎢0I000000A10O0A200O⎤⎦⎥⎥⎥∗dtAlin,kT=b_A_lin[12∗12]=[I0000I0000I0000I]+[00A1A2I00000O0000O]∗dt

/* 
开始根据计算过程方差 
*/ 

过程噪声方差b_q[12∗12]=⎡⎣⎢⎢⎢q00000q10000q20000q3⎤⎦⎥⎥⎥过程噪声方差b_q[12∗12]=[q00000q10000q20000q3]

其中各元素都是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];
      }
    }
  }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69
• 70
• 71
• 72
• 73
• 74
• 75
• 76
• 77
• 78
• 79
• 80
• 81
• 82
• 83
• 84
• 85
• 86
• 87
• 88
• 89
• 90
• 91
• 92
• 93
• 94
• 95
• 96
• 97
• 98
• 99
• 100
• 101
• 102
• 103
• 104
• 105
• 106
• 107
• 108
• 109
• 110
• 111
• 112
• 113
• 114
• 115
• 116
• 117
• 118
• 119
• 120
• 121
• 122
• 123
• 124
• 125
• 126
• 127
• 128
• 129
• 130
• 131
• 132
• 133
• 134
• 135
• 136
• 137
• 138
• 139
• 140
• 141
• 142
• 143
• 144
• 145
• 146
• 147
• 148
• 149
• 150
• 151
• 152
• 153
• 154
• 155
• 156
• 157
• 158
• 159
• 160
• 161
• 162
• 163
• 164
• 165
• 166
• 167
• 168
• 169

根据上面的矩阵运算，可以得到： 

A_lin[12∗12]=Pk[12∗12]∗bA_lin[12∗12];A_lin[12∗12]=Pk[12∗12]∗bA_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]=Alin,kPkAlin,kT;

c_A_lin[12∗12]=Pq[12∗12]∗bA_lin[12∗12];c_A_lin[12∗12]=Pq[12∗12]∗bA_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]=Alin,kQkAlin,kT;

  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];
    }
  } //最终过程方差

• 1
• 2
• 3
• 4
• 5

最终得到过程方差:： 

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];
        }
      }
    }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30

同样是计算了一堆中间矩阵： 

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]=HkPk;

其中：iv1[9∗12]=⎡⎣⎢I000000I000I⎤⎦⎥=Hk其中：iv1[9∗12]=[I00000I0000I]=Hk

k_k[12∗9]=P_apriori[12∗12]∗iv2[9∗12]=PkHTk;k_k[12∗9]=P_apriori[12∗12]∗iv2[9∗12]=PkHkT;

iv2[12∗9]=⎡⎣⎢⎢⎢I00000I0000I⎤⎦⎥⎥⎥=HTkiv2[12∗9]=[I000000I000I]=HkT

fv0[9∗9]=⎡⎣⎢I00000010001⎤⎦⎥∗k_k[12∗9]=HkPkHTkfv0[9∗9]=[I00000100001]∗k_k[12∗9]=HkPkHkT

测量噪声方差b_r[9∗9]=⎡⎣⎢r0000r1000r2⎤⎦⎥其中各元素都是3∗3的矩阵；测量噪声方差b_r[9∗9]=[r0000r1000r2]其中各元素都是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];
      }
    }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69
• 70
• 71
• 72
• 73
• 74
• 75
• 76
• 77
• 78
• 79
• 80
• 81
• 82
• 83
• 84
• 85
• 86

fv1[9∗9]=fv0[9∗9]+⎡⎣⎢r0000r1000r2⎤⎦⎥=HkPkHTk+Rfv1[9∗9]=fv0[9∗9]+[r0000r1000r2]=HkPkHkT+R

    /*矩 阵 除 法 ，计算出卡尔曼增益*/
    mrdivide(b_P_apriori, fv1, K_k);

• 1
• 2

这个函数的作用是计算卡尔曼增益：Kk[12∗9]T=K_K[9∗12]=b_P_apriori[9∗12]fv1[9∗9]这个函数的作用是计算卡尔曼增益：Kk[12∗9]T=K_K[9∗12]=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;
    }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18

计算状态后验估计： 

O[1∗9]=z[1∗9]−x_apriori[1∗12]∗HT[12∗9]O[1∗9]=z[1∗9]−x_apriori[1∗12]∗HT[12∗9]

得到:

x^k[12∗1]T=x_aposteriori[1∗12]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];
        }
      }
    }
  }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23

计算方差后验估计： 

b_A_lin[12∗12]=⎡⎣⎢⎢⎢I0000I0000I0000I⎤⎦⎥⎥⎥−HTk∗K_Kb_A_lin[12∗12]=[I0000I0000I0000I]−HkT∗K_K

得到:

Pk[12∗12]T=P_aposteriori[12∗12]Pk[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));
        }
      }
    }
  }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69
• 70
• 71
• 72
• 73
• 74
• 75
• 76
• 77
• 78
• 79
• 80
• 81
• 82
• 83
• 84
• 85
• 86
• 87
• 88
• 89
• 90
• 91
• 92
• 93
• 94
• 95
• 96
• 97
• 98
• 99
• 100
• 101
• 102
• 103
• 104
• 105
• 106
• 107
• 108
• 109
• 110
• 111
• 112
• 113
• 114
• 115
• 116
• 117
• 118
• 119
• 120
• 121
• 122
• 123
• 124
• 125
• 126
• 127
• 128
• 129
• 130
• 131
• 132
• 133
• 134
• 135
• 136
• 137
• 138
• 139
• 140
• 141
• 142
• 143
• 144
• 145
• 146
• 147
• 148
• 149
• 150
• 151
• 152
• 153
• 154
• 155
• 156
• 157
• 158
• 159
• 160
• 161
• 162
• 163
• 164
• 165
• 166
• 167
• 168
• 169
• 170
• 171
• 172
• 173
• 174
• 175
• 176
• 177
• 178
• 179
• 180
• 181
• 182
• 183
• 184
• 185
• 186
• 187
• 188
• 189
• 190
• 191
• 192
• 193
• 194
• 195
• 196
• 197
• 198
• 199
• 200
• 201
• 202
• 203
• 204
• 205
• 206
• 207
• 208
• 209
• 210
• 211
• 212
• 213
• 214
• 215
• 216
• 217
• 218
• 219
• 220
• 221
• 222
• 223
• 224
• 225
• 226
• 227
• 228
• 229
• 230
• 231
• 232
• 233
• 234
• 235
• 236
• 237
• 238
• 239
• 240
• 241
• 242
• 243
• 244
• 245
• 246
• 247
• 248
• 249
• 250
• 251
• 252
• 253
• 254
• 255
• 256
• 257
• 258
• 259
• 260
• 261
• 262
• 263
• 264
• 265
• 266
• 267
• 268
• 269
• 270
• 271
• 272
• 273
• 274
• 275
• 276
• 277
• 278
• 279
• 280
• 281
• 282
• 283
• 284
• 285
• 286
• 287
• 288
• 289
• 290
• 291
• 292
• 293
• 294
• 295
• 296
• 297
• 298
• 299
• 300
• 301
• 302
• 303
• 304
• 305
• 306
• 307
• 308
• 309
• 310
• 311
• 312
• 313
• 314
• 315
• 316
• 317
• 318
• 319
• 320
• 321
• 322
• 323
• 324
• 325
• 326
• 327
• 328
• 329
• 330
• 331
• 332
• 333
• 334
• 335
• 336
• 337
• 338
• 339
• 340
• 341
• 342
• 343
• 344
• 345
• 346
• 347
• 348
• 349
• 350
• 351
• 352
• 353
• 354
• 355
• 356
• 357
• 358
• 359
• 360
• 361
• 362
• 363
• 364
• 365
• 366
• 367
• 368
• 369
• 370
• 371
• 372
• 373
• 374
• 375
• 376
• 377
• 378
• 379
• 380
• 381
• 382
• 383

至此，EKF解算姿态过程全部结束，下面从姿态矩阵中提取欧拉角。其实本质就是计算新的余弦矩阵，然后根据下面的公式计算欧拉角 

Rot_matrix=⎡⎣⎢r0r3r6r1r4r7r2r5r8⎤⎦⎥Rot_matrix=[r0r1r2r3r4r5r6r7r8]

ϕ=arctan[r7r8]ϕ=arctan[r7r8]

θ=arcsin[−r6]θ=arcsin[−r6]

ψ=arcsin[r3r0]ψ=arcsin[r3r0]

  /* % 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) */

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54

3.下一步

把EKF估计姿态原理和具体算法细节搞清楚之后就可以移植到自己的工程上了，完成后把代码放上来。