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矩阵相当于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
/*
其实就是这个大家都很眼熟的的余弦矩阵的转置, 用来更新机体转过一个角度之后的重力和磁力三轴投影,只不过两次计算间隔时间很短,变化角度很小,因此忽略高阶小量之后就变成了这个样子。这里还少一个时间系数dt,下面会补上。
*/
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
/*
根据上述矩阵运算,可以得到:
从而:
从而:
其中
得到状态先验估计:
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矩阵:
其中各元素都是3*3的矩阵;I为单位矩阵,其中
同样
*/
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
得到:
/*
开始根据计算过程方差
*/
其中各元素都是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
根据上面的矩阵运算,可以得到:
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_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
/*矩 阵 除 法 ,计算出卡尔曼增益*/mrdivide(b_P_apriori, fv1, K_k);
- 1
- 2
/* 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
计算状态后验估计:
得到:
/* '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
计算方差后验估计:
得到:
到此就把所有的量都计算出来了!
下面几种情形为某个传感器未更新的情况,只需改变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解算姿态过程全部结束,下面从姿态矩阵中提取欧拉角。其实本质就是计算新的余弦矩阵,然后根据下面的公式计算欧拉角
/* % 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估计姿态原理和具体算法细节搞清楚之后就可以移植到自己的工程上了,完成后把代码放上来。
PX4飞控中利用EKF估计姿态角代码详解相关推荐
- js 浅拷贝直接赋值_JS中实现浅拷贝和深拷贝的代码详解
(一)JS中基本类型和引用类型 JavaScript的变量中包含两种类型的值:基本类型值 和 引用类型值,在内存中的表现形式在于:前者是存储在栈中的一些简单的数据段,后者则是保存在堆内存中的一个对象. ...
- 【Java】Java中的设计模式的介绍以及代码详解
一.什么是设计模式? 设计模式是软件开发人员在软件开发过程中面临的一般问题的解决方案.这些解决方案是众多软件开发人员经过相当长的一段时间的试验和错误 总结出来的. 对问题行之有效地解决方式,是一种设计 ...
- C++11中的一些新特性以及代码详解
C++11新特性 auto decltype 追踪返回类型 类内成员初始化 列表初始化 基于范围的for循环 静态断言 noexcept修饰符 强类型枚举 常量表达式 原生字符串字面值 继承控制 fi ...
- html中输出PHP的下拉列表,html中关于下拉列表select的图文代码详解
HTML中的下拉列表: Html代码 Volvo Saab Opel Audi 其中select是显示一个下拉列表(drop down list)出来,option是下拉列表中的项目(item),而o ...
- JAVA程序设计计时器代码_Java中的定时器Timer使用示例代码详解
一.类概述 Timer是一种定时器工具,用来在一个后台线程计划执行指定任务.它可以计划执行一个任务一次或反复多次. TimerTask一个抽象类,它的子类代表一个可以被Timer计划的任务. 二.代码 ...
- java 代码压缩javascript_利用Java来压缩 JavaScript 代码详解
通过移除空行和注释来压缩 JavaScript 代码 /** * This file is part of the Echo Web Application Framework (hereinafte ...
- v9php 碎片信息,PHPCMS V9模板中的常用变量、碎片代码详解
前面是变量,后面是调用变量的解释 {pc:content action="position"posid="12" thumb="1" ord ...
- 人体姿态估计HRNet网络模型搭建代码详解
HRNet-v1模型详解 源码参考:https://github.com/HRNet/HRNet-Human-Pose-Estimation 内容参考:点击跳转 仅作为个人的学习笔记,欢迎交流学习. ...
- 利用unity和steamVR完成场景漫游(二) 关于steamVR插件中的代码详解
1.SteamVR.cs 单例管理类,管理SteamVR程序的运行和终止. 2.SteamVR_Camera.cs 给场景添加一个最基本可运行的SteamVR组. 3.SteamVR_CameraFl ...
最新文章
- CF 8D Two Friends (三分+二分)
- The Excel Connection Manager is not supported in the 64-bit version of SSIS, as no OLE DB provider i
- 警惕技术人员的极端性
- artDiaLog弹出插件
- dotNET Core 3.X 请求处理管道和中间件的理解
- 红旗linux 进不去图形界面,进不了红旗Linux6.0的图形界面请高手帮忙
- 前端vscode插件合集
- 将本地代码上传至github
- 在CSS中使用not:first-child选择器
- 卷积神经网络_mnist
- LeetCode之Find Eventual Safe States(Kotlin)
- UI设计师经常去的五个网站
- 易宝支付(Java实现)
- mathpix安装和使用详细教程
- 合唱队形(c++DP)
- 联想win10进bios的正确方式,并不是按键!!!!!
- 联想台式计算机HDMI使用,联想电脑怎样连接电视
- 会动的博物馆?广州华锐互动3D展示技术实现空间复刻
- 用WIN汇编开发桌面报时工具
- 之江实验室与Science《科学》联合发布智能计算领域十大科学问题