Alberto Petrucci / Mbed 2 deprecated IMU-9DOF-KALMAN

Simple example to show how to get an estimation of the attitude with a 9DOF IMU and the Kalman filter

Dependencies:   L3GD20 LSM303DLHC mbed-dsp mbed

Fork of minimu-9v2 by brian claus

sensfusion9.c@1:ba2d31e3112d, 2017-03-25 (annotated)

Committer:
capriele
Date:
Sat Mar 25 16:48:32 2017 +0000
Revision:
1:ba2d31e3112d
Simple example to get an estimation of the attitude thought a 9DOF IMU and the Kalman filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
capriele 1:ba2d31e3112d 1 #include "sensfusion9.h"
capriele 1:ba2d31e3112d 2 #include "math.h"
capriele 1:ba2d31e3112d 3 #include "arm_math.h"
capriele 1:ba2d31e3112d 4
capriele 1:ba2d31e3112d 5 #define M_PI_F 3.14159265358979323846f /* pi */
capriele 1:ba2d31e3112d 6
capriele 1:ba2d31e3112d 7 static inline void mat_trans(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst)
capriele 1:ba2d31e3112d 8 {
capriele 1:ba2d31e3112d 9 arm_mat_trans_f32(pSrc, pDst);
capriele 1:ba2d31e3112d 10 }
capriele 1:ba2d31e3112d 11 static inline void mat_inv(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst)
capriele 1:ba2d31e3112d 12 {
capriele 1:ba2d31e3112d 13 arm_mat_inverse_f32(pSrc, pDst);
capriele 1:ba2d31e3112d 14 }
capriele 1:ba2d31e3112d 15 static inline void mat_mult(const arm_matrix_instance_f32 * pSrcA, const arm_matrix_instance_f32 * pSrcB, arm_matrix_instance_f32 * pDst)
capriele 1:ba2d31e3112d 16 {
capriele 1:ba2d31e3112d 17 arm_mat_mult_f32(pSrcA, pSrcB, pDst);
capriele 1:ba2d31e3112d 18 }
capriele 1:ba2d31e3112d 19 static inline float arm_sqrt(float32_t in)
capriele 1:ba2d31e3112d 20 {
capriele 1:ba2d31e3112d 21 float pOut = 0;
capriele 1:ba2d31e3112d 22 arm_status result = arm_sqrt_f32(in, &pOut);
capriele 1:ba2d31e3112d 23 return pOut;
capriele 1:ba2d31e3112d 24 }
capriele 1:ba2d31e3112d 25
capriele 1:ba2d31e3112d 26 #define STATE_SIZE 4
capriele 1:ba2d31e3112d 27 #define OUTPUT_SIZE 6
capriele 1:ba2d31e3112d 28 static float Q_VARIANCE = 0.01f;
capriele 1:ba2d31e3112d 29 static float R_VARIANCE_ACC = 25.0f;
capriele 1:ba2d31e3112d 30 static float R_VARIANCE_MAG = 50.0f;
capriele 1:ba2d31e3112d 31
capriele 1:ba2d31e3112d 32 // The covariance matrix
capriele 1:ba2d31e3112d 33 static float P[STATE_SIZE][STATE_SIZE] = {{0}};
capriele 1:ba2d31e3112d 34 static float R[OUTPUT_SIZE] = {0};
capriele 1:ba2d31e3112d 35 static arm_matrix_instance_f32 Pm = {STATE_SIZE, STATE_SIZE, (float *)P};
capriele 1:ba2d31e3112d 36
capriele 1:ba2d31e3112d 37 // The state update matrix
capriele 1:ba2d31e3112d 38 static float A[STATE_SIZE][STATE_SIZE];
capriele 1:ba2d31e3112d 39 static arm_matrix_instance_f32 Am = {STATE_SIZE, STATE_SIZE, (float *)A}; // linearized dynamics for covariance update;
capriele 1:ba2d31e3112d 40
capriele 1:ba2d31e3112d 41 // Temporary matrices for the covariance updates
capriele 1:ba2d31e3112d 42 static float tmpNN1d[STATE_SIZE][STATE_SIZE];
capriele 1:ba2d31e3112d 43 static arm_matrix_instance_f32 tmpNN1m = { STATE_SIZE, STATE_SIZE, (float *)tmpNN1d};
capriele 1:ba2d31e3112d 44
capriele 1:ba2d31e3112d 45 static float tmpNN2d[STATE_SIZE][STATE_SIZE];
capriele 1:ba2d31e3112d 46 static arm_matrix_instance_f32 tmpNN2m = { STATE_SIZE, STATE_SIZE, (float *)tmpNN2d};
capriele 1:ba2d31e3112d 47
capriele 1:ba2d31e3112d 48 // quaternion of sensor frame relative to auxiliary frame
capriele 1:ba2d31e3112d 49 static float q0 = 1.0f;
capriele 1:ba2d31e3112d 50 static float q1 = 0.0f;
capriele 1:ba2d31e3112d 51 static float q2 = 0.0f;
capriele 1:ba2d31e3112d 52 static float q3 = 0.0f;
capriele 1:ba2d31e3112d 53
capriele 1:ba2d31e3112d 54 // Unit vector in the estimated gravity direction
capriele 1:ba2d31e3112d 55 static float gravX, gravY, gravZ;
capriele 1:ba2d31e3112d 56
capriele 1:ba2d31e3112d 57 // The acc in Z for static position (g)
capriele 1:ba2d31e3112d 58 // Set on first update, assuming we are in a static position since the sensors were just calibrates.
capriele 1:ba2d31e3112d 59 // This value will be better the more level the copter is at calibration time
capriele 1:ba2d31e3112d 60 static float baseZacc = 1.0;
capriele 1:ba2d31e3112d 61
capriele 1:ba2d31e3112d 62 static bool isInit;
capriele 1:ba2d31e3112d 63
capriele 1:ba2d31e3112d 64 static bool isCalibrated = false;
capriele 1:ba2d31e3112d 65
capriele 1:ba2d31e3112d 66 static void sensfusion9UpdateQImpl(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt);
capriele 1:ba2d31e3112d 67 static float sensfusion9GetAccZ(const float ax, const float ay, const float az);
capriele 1:ba2d31e3112d 68 static void estimatedGravityDirection(float* gx, float* gy, float* gz);
capriele 1:ba2d31e3112d 69
capriele 1:ba2d31e3112d 70 // TODO: Make math util file
capriele 1:ba2d31e3112d 71 static float invSqrt(float x);
capriele 1:ba2d31e3112d 72
capriele 1:ba2d31e3112d 73 void sensfusion9Init()
capriele 1:ba2d31e3112d 74 {
capriele 1:ba2d31e3112d 75 if(isInit)
capriele 1:ba2d31e3112d 76 return;
capriele 1:ba2d31e3112d 77 for(int i = 0; i < STATE_SIZE; ++i)
capriele 1:ba2d31e3112d 78 P[i][i] = Q_VARIANCE;
capriele 1:ba2d31e3112d 79 R[0] = R_VARIANCE_ACC;
capriele 1:ba2d31e3112d 80 R[1] = R_VARIANCE_ACC;
capriele 1:ba2d31e3112d 81 R[2] = R_VARIANCE_ACC;
capriele 1:ba2d31e3112d 82 R[3] = R_VARIANCE_MAG;
capriele 1:ba2d31e3112d 83 R[4] = R_VARIANCE_MAG;
capriele 1:ba2d31e3112d 84 R[5] = R_VARIANCE_MAG;
capriele 1:ba2d31e3112d 85 isInit = true;
capriele 1:ba2d31e3112d 86 }
capriele 1:ba2d31e3112d 87
capriele 1:ba2d31e3112d 88 bool sensfusion9Test(void)
capriele 1:ba2d31e3112d 89 {
capriele 1:ba2d31e3112d 90 return isInit;
capriele 1:ba2d31e3112d 91 }
capriele 1:ba2d31e3112d 92
capriele 1:ba2d31e3112d 93 void sensfusion9UpdateQ(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
capriele 1:ba2d31e3112d 94 {
capriele 1:ba2d31e3112d 95 //Cambio gli assi del magnetometro perchè rispetto a quelli del gyro/acc sono diversi:
capriele 1:ba2d31e3112d 96 //In particolare:
capriele 1:ba2d31e3112d 97 //Mx = (Gy = Ay)
capriele 1:ba2d31e3112d 98 //My = (Gx = Ax)
capriele 1:ba2d31e3112d 99 //-Mz = (Gz = Az)
capriele 1:ba2d31e3112d 100 sensfusion9UpdateQImpl(gx, gy, gz, ax, ay, az, mx, my, mz, dt);
capriele 1:ba2d31e3112d 101 estimatedGravityDirection(&gravX, &gravY, &gravZ);
capriele 1:ba2d31e3112d 102 if (!isCalibrated) {
capriele 1:ba2d31e3112d 103 baseZacc = sensfusion9GetAccZ(ax, ay, az);
capriele 1:ba2d31e3112d 104 isCalibrated = true;
capriele 1:ba2d31e3112d 105 }
capriele 1:ba2d31e3112d 106 }
capriele 1:ba2d31e3112d 107
capriele 1:ba2d31e3112d 108 /*
capriele 1:ba2d31e3112d 109 static void sensfusion9UpdateQImpl(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
capriele 1:ba2d31e3112d 110 {
capriele 1:ba2d31e3112d 111 float recipNorm;
capriele 1:ba2d31e3112d 112
capriele 1:ba2d31e3112d 113 gx = gx * M_PI_F / 180.0f;
capriele 1:ba2d31e3112d 114 gy = gy * M_PI_F / 180.0f;
capriele 1:ba2d31e3112d 115 gz = gz * M_PI_F / 180.0f;
capriele 1:ba2d31e3112d 116
capriele 1:ba2d31e3112d 117 // Normalise accelerometer measurement
capriele 1:ba2d31e3112d 118 recipNorm = invSqrt(ax*ax + ay*ay + az*az);
capriele 1:ba2d31e3112d 119 ax *= recipNorm;
capriele 1:ba2d31e3112d 120 ay *= recipNorm;
capriele 1:ba2d31e3112d 121 az *= recipNorm;
capriele 1:ba2d31e3112d 122
capriele 1:ba2d31e3112d 123 // Normalise magnetometer measurement
capriele 1:ba2d31e3112d 124 recipNorm = invSqrt(mx*mx + my*my + mz*mz);
capriele 1:ba2d31e3112d 125 mx *= recipNorm;
capriele 1:ba2d31e3112d 126 my *= recipNorm;
capriele 1:ba2d31e3112d 127 mz *= recipNorm;
capriele 1:ba2d31e3112d 128
capriele 1:ba2d31e3112d 129 A[0][0] = 1.0f;
capriele 1:ba2d31e3112d 130 A[0][1] = gz*dt;
capriele 1:ba2d31e3112d 131 A[0][2] = -gy*dt;
capriele 1:ba2d31e3112d 132 A[1][0] = -gz*dt;
capriele 1:ba2d31e3112d 133 A[1][1] = 1.0f;
capriele 1:ba2d31e3112d 134 A[1][2] = gx*dt;
capriele 1:ba2d31e3112d 135 A[2][0] = gy*dt;
capriele 1:ba2d31e3112d 136 A[2][1] = -gx*dt;
capriele 1:ba2d31e3112d 137 A[2][2] = 1.0f;
capriele 1:ba2d31e3112d 138
capriele 1:ba2d31e3112d 139 float q[4][1];
capriele 1:ba2d31e3112d 140 arm_matrix_instance_f32 qm = {4, 1, (float *)q};
capriele 1:ba2d31e3112d 141 q[0][0] = q0;
capriele 1:ba2d31e3112d 142 q[1][0] = q1;
capriele 1:ba2d31e3112d 143 q[2][0] = q2;
capriele 1:ba2d31e3112d 144 q[3][0] = q3;
capriele 1:ba2d31e3112d 145
capriele 1:ba2d31e3112d 146 float W[4][4];
capriele 1:ba2d31e3112d 147 arm_matrix_instance_f32 Wm = {4, 4, (float *)W};
capriele 1:ba2d31e3112d 148 W[0][0] = 0.0f;
capriele 1:ba2d31e3112d 149 W[0][1] = -gx;
capriele 1:ba2d31e3112d 150 W[0][2] = -gy;
capriele 1:ba2d31e3112d 151 W[0][3] = -gz;
capriele 1:ba2d31e3112d 152 W[1][0] = gx;
capriele 1:ba2d31e3112d 153 W[1][1] = 0.0f;
capriele 1:ba2d31e3112d 154 W[1][2] = gz;
capriele 1:ba2d31e3112d 155 W[1][3] = -gy;
capriele 1:ba2d31e3112d 156 W[2][0] = gy;
capriele 1:ba2d31e3112d 157 W[2][1] = -gz;
capriele 1:ba2d31e3112d 158 W[2][2] = 0.0f;
capriele 1:ba2d31e3112d 159 W[2][3] = gx;
capriele 1:ba2d31e3112d 160 W[3][0] = gz;
capriele 1:ba2d31e3112d 161 W[3][1] = gy;
capriele 1:ba2d31e3112d 162 W[3][2] = -gx;
capriele 1:ba2d31e3112d 163 W[3][3] = 0.0f;
capriele 1:ba2d31e3112d 164
capriele 1:ba2d31e3112d 165 //Aggiorno lo stato con runge kutta 4
capriele 1:ba2d31e3112d 166 float m1[4][1];
capriele 1:ba2d31e3112d 167 float m2[4][1];
capriele 1:ba2d31e3112d 168 float m3[4][1];
capriele 1:ba2d31e3112d 169 float x1[4][1];
capriele 1:ba2d31e3112d 170 float x2[4][1];
capriele 1:ba2d31e3112d 171 arm_matrix_instance_f32 m1m = {4, 1, (float *)m1};
capriele 1:ba2d31e3112d 172 arm_matrix_instance_f32 m2m = {4, 1, (float *)m2};
capriele 1:ba2d31e3112d 173 arm_matrix_instance_f32 m3m = {4, 1, (float *)m3};
capriele 1:ba2d31e3112d 174 arm_matrix_instance_f32 x1m = {4, 1, (float *)x1};
capriele 1:ba2d31e3112d 175 arm_matrix_instance_f32 x2m = {4, 1, (float *)x2};
capriele 1:ba2d31e3112d 176
capriele 1:ba2d31e3112d 177 //Primo step
capriele 1:ba2d31e3112d 178 mat_mult(&Wm, &qm, &m1m);
capriele 1:ba2d31e3112d 179 for(int i = 0; i < 4; ++i){
capriele 1:ba2d31e3112d 180 x1[i][0] = q[i][0] + 0.5f*m1[i][0]*dt;
capriele 1:ba2d31e3112d 181 }
capriele 1:ba2d31e3112d 182
capriele 1:ba2d31e3112d 183 //Secondo step
capriele 1:ba2d31e3112d 184 mat_mult(&Wm, &x1m, &m2m);
capriele 1:ba2d31e3112d 185 for(int i = 0; i < 4; ++i){
capriele 1:ba2d31e3112d 186 x2[i][0] = q[i][0] + 0.5f*(m1[i][0] + m2[i][0])*dt/4.0f;
capriele 1:ba2d31e3112d 187 }
capriele 1:ba2d31e3112d 188
capriele 1:ba2d31e3112d 189 //Terzo step
capriele 1:ba2d31e3112d 190 mat_mult(&Wm, &x2m, &m3m);
capriele 1:ba2d31e3112d 191 for(int i = 0; i < 4; ++i){
capriele 1:ba2d31e3112d 192 q[i][0] = q[i][0] + 0.5f*(m1[i][0] + m2[i][0] + 4.0f*m3[i][0])*(dt/6.0f);
capriele 1:ba2d31e3112d 193 }
capriele 1:ba2d31e3112d 194
capriele 1:ba2d31e3112d 195 //Normalizzo la stima dei quaternioni
capriele 1:ba2d31e3112d 196 recipNorm = invSqrt(q[0][0]*q[0][0] + q[1][0]*q[1][0] + q[2][0]*q[2][0] + q[3][0]*q[3][0]);
capriele 1:ba2d31e3112d 197 for(int i = 0; i < 4; ++i){
capriele 1:ba2d31e3112d 198 q[i][0] *= recipNorm;
capriele 1:ba2d31e3112d 199 }
capriele 1:ba2d31e3112d 200
capriele 1:ba2d31e3112d 201 q0 = q[0][0];
capriele 1:ba2d31e3112d 202 q1 = q[1][0];
capriele 1:ba2d31e3112d 203 q2 = q[2][0];
capriele 1:ba2d31e3112d 204 q3 = q[3][0];
capriele 1:ba2d31e3112d 205
capriele 1:ba2d31e3112d 206 //Matrice di rotazione
capriele 1:ba2d31e3112d 207 float QuaternionDCM[3][3];
capriele 1:ba2d31e3112d 208 arm_matrix_instance_f32 QuaternionDCMm = {3, 3, (float *)QuaternionDCM};
capriele 1:ba2d31e3112d 209 QuaternionDCM[0][0] = 2*q0*q0-1+2*q1*q1;
capriele 1:ba2d31e3112d 210 QuaternionDCM[0][1] = 2*(q1*q2+q0*q3);
capriele 1:ba2d31e3112d 211 QuaternionDCM[0][2] = 2*(q1*q3-q0*q2);
capriele 1:ba2d31e3112d 212 QuaternionDCM[1][0] = 2*(q1*q2-q0*q3);
capriele 1:ba2d31e3112d 213 QuaternionDCM[1][1] = 2*q0*q0-1+2*q2*q2;
capriele 1:ba2d31e3112d 214 QuaternionDCM[1][2] = 2*(q2*q3+q0*q1);
capriele 1:ba2d31e3112d 215 QuaternionDCM[2][0] = 2*(q1*q3+q0*q2);
capriele 1:ba2d31e3112d 216 QuaternionDCM[2][1] = 2*(q2*q3-q0*q1);
capriele 1:ba2d31e3112d 217 QuaternionDCM[2][2] = 2*q0*q0-1+2*q3*q3;
capriele 1:ba2d31e3112d 218
capriele 1:ba2d31e3112d 219 //Gravity Reference
capriele 1:ba2d31e3112d 220 float G[3][1];
capriele 1:ba2d31e3112d 221 float Gr[3][1];
capriele 1:ba2d31e3112d 222 arm_matrix_instance_f32 Gm = {3, 1, (float *)G};
capriele 1:ba2d31e3112d 223 arm_matrix_instance_f32 Grm = {3, 1, (float *)Gr};
capriele 1:ba2d31e3112d 224 G[0][0] = 0;
capriele 1:ba2d31e3112d 225 G[1][0] = 0;
capriele 1:ba2d31e3112d 226 G[2][0] = 1;
capriele 1:ba2d31e3112d 227 mat_mult(&QuaternionDCMm, &Gm, &Grm);
capriele 1:ba2d31e3112d 228
capriele 1:ba2d31e3112d 229 //Magnetic field Reference
capriele 1:ba2d31e3112d 230 float M[3][1];
capriele 1:ba2d31e3112d 231 float Mr[3][1];
capriele 1:ba2d31e3112d 232 arm_matrix_instance_f32 Mm = {3, 1, (float *)M};
capriele 1:ba2d31e3112d 233 arm_matrix_instance_f32 Mrm = {3, 1, (float *)Mr};
capriele 1:ba2d31e3112d 234 float hx = mx*q0*q0 + 2.0f*mz*q0*q2 - 2.0f*my*q0*q3 + mx*q1*q1 + 2.0f*my*q1*q2 + 2.0f*mz*q1*q3 - mx*q2*q2 - mx*q3*q3;
capriele 1:ba2d31e3112d 235 float hy = my*q0*q0 - 2.0f*mz*q0*q1 + 2.0f*mx*q0*q3 - my*q1*q1 + 2.0f*mx*q1*q2 + my*q2*q2 + 2.0f*mz*q2*q3 - my*q3*q3;
capriele 1:ba2d31e3112d 236 float hz = mz*q0*q0 + 2.0f*my*q0*q1 - 2.0f*mx*q0*q2 - mz*q1*q1 + 2.0f*mx*q1*q3 - mz*q2*q1 + 2.0f*my*q2*q3 + mz*q3*q3;
capriele 1:ba2d31e3112d 237 M[0][0] = arm_sqrt(hx*hx + hy*hy);
capriele 1:ba2d31e3112d 238 M[1][0] = 0;
capriele 1:ba2d31e3112d 239 M[2][0] = hz;
capriele 1:ba2d31e3112d 240 mat_mult(&QuaternionDCMm, &Mm, &Mrm);
capriele 1:ba2d31e3112d 241
capriele 1:ba2d31e3112d 242 // Aggiorno la Covarianza
capriele 1:ba2d31e3112d 243 float P0[STATE_SIZE][STATE_SIZE];
capriele 1:ba2d31e3112d 244 arm_matrix_instance_f32 P0m = {STATE_SIZE, STATE_SIZE, (float *)P0};
capriele 1:ba2d31e3112d 245
capriele 1:ba2d31e3112d 246 mat_mult(&Am, &Pm, &tmpNN1m); // A P
capriele 1:ba2d31e3112d 247 mat_trans(&Am, &tmpNN2m); // A'
capriele 1:ba2d31e3112d 248 mat_mult(&tmpNN1m, &tmpNN2m, &P0m); // A P A'
capriele 1:ba2d31e3112d 249 for(int i = 0; i < STATE_SIZE; ++i){
capriele 1:ba2d31e3112d 250 P0[i][i] += Q_VARIANCE;
capriele 1:ba2d31e3112d 251 }
capriele 1:ba2d31e3112d 252
capriele 1:ba2d31e3112d 253 // Calcolo il guadagno
capriele 1:ba2d31e3112d 254 float K[STATE_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 255 arm_matrix_instance_f32 Km = {STATE_SIZE, OUTPUT_SIZE, (float *)K};
capriele 1:ba2d31e3112d 256
capriele 1:ba2d31e3112d 257 float h[OUTPUT_SIZE][STATE_SIZE];
capriele 1:ba2d31e3112d 258 arm_matrix_instance_f32 Hm = {OUTPUT_SIZE, STATE_SIZE, (float *)h};
capriele 1:ba2d31e3112d 259
capriele 1:ba2d31e3112d 260 float hT[STATE_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 261 arm_matrix_instance_f32 HTm = {STATE_SIZE, OUTPUT_SIZE, (float *)hT};
capriele 1:ba2d31e3112d 262
capriele 1:ba2d31e3112d 263 float P0hT[STATE_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 264 arm_matrix_instance_f32 P0HTm = {STATE_SIZE, OUTPUT_SIZE, (float *)P0hT};
capriele 1:ba2d31e3112d 265
capriele 1:ba2d31e3112d 266 float hP0hT[OUTPUT_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 267 arm_matrix_instance_f32 HP0HTm = {OUTPUT_SIZE, OUTPUT_SIZE, (float *)hP0hT};
capriele 1:ba2d31e3112d 268
capriele 1:ba2d31e3112d 269 float hP0hT_inv[OUTPUT_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 270 arm_matrix_instance_f32 HP0HT_INVm = {OUTPUT_SIZE, OUTPUT_SIZE, (float *)hP0hT_inv};
capriele 1:ba2d31e3112d 271
capriele 1:ba2d31e3112d 272 //Accelerometer
capriele 1:ba2d31e3112d 273 h[0][0] = 0;
capriele 1:ba2d31e3112d 274 h[0][1] = -Gr[2][0];
capriele 1:ba2d31e3112d 275 h[0][2] = Gr[1][0];
capriele 1:ba2d31e3112d 276 h[1][0] = Gr[2][0];
capriele 1:ba2d31e3112d 277 h[1][1] = 0;
capriele 1:ba2d31e3112d 278 h[1][2] = -Gr[0][0];
capriele 1:ba2d31e3112d 279 h[2][0] = -Gr[1][0];
capriele 1:ba2d31e3112d 280 h[2][1] = Gr[0][0];
capriele 1:ba2d31e3112d 281 h[2][2] = 0;
capriele 1:ba2d31e3112d 282
capriele 1:ba2d31e3112d 283 //Magnetometer
capriele 1:ba2d31e3112d 284 h[3][0] = 0;
capriele 1:ba2d31e3112d 285 h[3][1] = -Mr[2][0];
capriele 1:ba2d31e3112d 286 h[3][2] = Mr[1][0];
capriele 1:ba2d31e3112d 287 h[4][0] = Mr[2][0];
capriele 1:ba2d31e3112d 288 h[4][1] = 0;
capriele 1:ba2d31e3112d 289 h[4][2] = -Mr[0][0];
capriele 1:ba2d31e3112d 290 h[5][0] = -Mr[1][0];
capriele 1:ba2d31e3112d 291 h[5][1] = Mr[0][0];
capriele 1:ba2d31e3112d 292 h[5][2] = 0;
capriele 1:ba2d31e3112d 293
capriele 1:ba2d31e3112d 294
capriele 1:ba2d31e3112d 295 // ====== INNOVATION COVARIANCE ======
capriele 1:ba2d31e3112d 296 mat_trans(&Hm, &HTm);
capriele 1:ba2d31e3112d 297 mat_mult(&P0m, &HTm, &P0HTm); // P0*H'
capriele 1:ba2d31e3112d 298 mat_mult(&Hm, &P0HTm, &HP0HTm); // H*P0*H'
capriele 1:ba2d31e3112d 299 for (int i = 0; i < OUTPUT_SIZE; ++i) { // Add the element of HPH' to the above
capriele 1:ba2d31e3112d 300 hP0hT[i][i] += R[i];
capriele 1:ba2d31e3112d 301 }
capriele 1:ba2d31e3112d 302 mat_inv(&HP0HTm, &HP0HT_INVm); // (H*P0*H' + R)^(-1)
capriele 1:ba2d31e3112d 303 mat_mult(&P0HTm, &HP0HT_INVm, &Km); // K = P0*H'*(H*P0*H' + R)^(-1)
capriele 1:ba2d31e3112d 304
capriele 1:ba2d31e3112d 305 //Aggiorno la P
capriele 1:ba2d31e3112d 306 mat_mult(&Km, &Hm, &tmpNN1m); // KH
capriele 1:ba2d31e3112d 307 for (int i = 0; i < STATE_SIZE; ++i) {
capriele 1:ba2d31e3112d 308 tmpNN1d[i][i] -= 1.0f;
capriele 1:ba2d31e3112d 309 for (int j = 0; j < STATE_SIZE; ++j) {
capriele 1:ba2d31e3112d 310 tmpNN1d[i][j] *= -1.0f;
capriele 1:ba2d31e3112d 311 }
capriele 1:ba2d31e3112d 312 } // -(KH - I)
capriele 1:ba2d31e3112d 313 mat_mult(&tmpNN1m, &P0m, &Pm); // -(KH - I)*P0
capriele 1:ba2d31e3112d 314
capriele 1:ba2d31e3112d 315 float Error[OUTPUT_SIZE][1];
capriele 1:ba2d31e3112d 316 arm_matrix_instance_f32 Errorm = {STATE_SIZE, 1, (float *)Error};
capriele 1:ba2d31e3112d 317 float ae[STATE_SIZE][1];
capriele 1:ba2d31e3112d 318 arm_matrix_instance_f32 aem = {STATE_SIZE, 1, (float *)ae};
capriele 1:ba2d31e3112d 319 Error[0][0] = ax - Gr[0][0];
capriele 1:ba2d31e3112d 320 Error[1][0] = ay - Gr[1][0];
capriele 1:ba2d31e3112d 321 Error[2][0] = az - Gr[2][0];
capriele 1:ba2d31e3112d 322 Error[3][0] = mx - Mr[0][0];
capriele 1:ba2d31e3112d 323 Error[4][0] = my - Mr[1][0];
capriele 1:ba2d31e3112d 324 Error[5][0] = mz - Mr[2][0];
capriele 1:ba2d31e3112d 325 mat_mult(&Km, &Errorm, &aem); // K*error(k)
capriele 1:ba2d31e3112d 326
capriele 1:ba2d31e3112d 327 float qe0 = 1;
capriele 1:ba2d31e3112d 328 float qe1 = ae[0][0]/2;
capriele 1:ba2d31e3112d 329 float qe2 = ae[1][0]/2;
capriele 1:ba2d31e3112d 330 float qe3 = ae[2][0]/2;
capriele 1:ba2d31e3112d 331
capriele 1:ba2d31e3112d 332 float q0tmp = q0;
capriele 1:ba2d31e3112d 333 float q1tmp = q1;
capriele 1:ba2d31e3112d 334 float q2tmp = q2;
capriele 1:ba2d31e3112d 335 float q3tmp = q3;
capriele 1:ba2d31e3112d 336 q0 = q0tmp*qe0 - q1tmp*qe1 - q2tmp*qe2 - q3tmp*qe3;
capriele 1:ba2d31e3112d 337 q1 = q0tmp*qe1 + q1tmp*qe0 + q2tmp*qe3 - q3tmp*qe2;
capriele 1:ba2d31e3112d 338 q2 = q0tmp*qe2 + q2tmp*qe0 - q1tmp*qe3 + q3tmp*qe1;
capriele 1:ba2d31e3112d 339 q3 = q0tmp*qe3 + q1tmp*qe2 - q2tmp*qe1 + q3tmp*qe0;
capriele 1:ba2d31e3112d 340
capriele 1:ba2d31e3112d 341 // Normalise quaternion
capriele 1:ba2d31e3112d 342 recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
capriele 1:ba2d31e3112d 343 q0 *= recipNorm;
capriele 1:ba2d31e3112d 344 q1 *= recipNorm;
capriele 1:ba2d31e3112d 345 q2 *= recipNorm;
capriele 1:ba2d31e3112d 346 q3 *= recipNorm;
capriele 1:ba2d31e3112d 347 }
capriele 1:ba2d31e3112d 348 */
capriele 1:ba2d31e3112d 349 static void sensfusion9UpdateQImpl(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
capriele 1:ba2d31e3112d 350 {
capriele 1:ba2d31e3112d 351 float recipNorm;
capriele 1:ba2d31e3112d 352
capriele 1:ba2d31e3112d 353 gx = gx * M_PI_F / 180.0f;
capriele 1:ba2d31e3112d 354 gy = gy * M_PI_F / 180.0f;
capriele 1:ba2d31e3112d 355 gz = gz * M_PI_F / 180.0f;
capriele 1:ba2d31e3112d 356
capriele 1:ba2d31e3112d 357 // Normalise accelerometer measurement
capriele 1:ba2d31e3112d 358 recipNorm = invSqrt(ax*ax + ay*ay + az*az);
capriele 1:ba2d31e3112d 359 ax *= recipNorm;
capriele 1:ba2d31e3112d 360 ay *= recipNorm;
capriele 1:ba2d31e3112d 361 az *= recipNorm;
capriele 1:ba2d31e3112d 362
capriele 1:ba2d31e3112d 363 // Normalise magnetometer measurement
capriele 1:ba2d31e3112d 364 recipNorm = invSqrt(mx*mx + my*my + mz*mz);
capriele 1:ba2d31e3112d 365 mx *= recipNorm;
capriele 1:ba2d31e3112d 366 my *= recipNorm;
capriele 1:ba2d31e3112d 367 mz *= recipNorm;
capriele 1:ba2d31e3112d 368
capriele 1:ba2d31e3112d 369 //Converto il vettore intensità campo magnetico
capriele 1:ba2d31e3112d 370 //float h1 = mx*q1 + my*q2 + mz*q3;
capriele 1:ba2d31e3112d 371 //float h2 = mx*q0 - my*q3 + mz*q2;
capriele 1:ba2d31e3112d 372 //float h3 = mx*q3 + my*q0 - mz*q1;
capriele 1:ba2d31e3112d 373 //float h4 = -mx*q2 + my*q1 + mz*q0;
capriele 1:ba2d31e3112d 374 //float n2 = q0*h2 + q1*h1 + q2*h4 - q3*h3;
capriele 1:ba2d31e3112d 375 //float n3 = q0*h3 - q1*h4 + q2*h1 + q3*h2;
capriele 1:ba2d31e3112d 376 //float n4 = q0*h4 + q1*h3 - q2*h2 + q3*h1;
capriele 1:ba2d31e3112d 377 //float b2 = arm_sqrt(n2*n2 + n3*n3);
capriele 1:ba2d31e3112d 378 //float b4 = n4;
capriele 1:ba2d31e3112d 379 //float b2 = mx;
capriele 1:ba2d31e3112d 380 //float b4 = mz;
capriele 1:ba2d31e3112d 381 float hx = mx*q0*q0 + 2.0f*mz*q0*q2 - 2.0f*my*q0*q3 + mx*q1*q1 + 2.0f*my*q1*q2 + 2.0f*mz*q1*q3 - mx*q2*q2 - mx*q3*q3;
capriele 1:ba2d31e3112d 382 float hy = my*q0*q0 - 2.0f*mz*q0*q1 + 2.0f*mx*q0*q3 - my*q1*q1 + 2.0f*mx*q1*q2 + my*q2*q2 + 2.0f*mz*q2*q3 - my*q3*q3;
capriele 1:ba2d31e3112d 383 float hz = mz*q0*q0 + 2.0f*my*q0*q1 - 2.0f*mx*q0*q2 - mz*q1*q1 + 2.0f*mx*q1*q3 - mz*q2*q1 + 2.0f*my*q2*q3 + mz*q3*q3;
capriele 1:ba2d31e3112d 384 float b2 = arm_sqrt(hx*hx + hy*hy);
capriele 1:ba2d31e3112d 385 float b4 = hz;
capriele 1:ba2d31e3112d 386
capriele 1:ba2d31e3112d 387 A[0][0] = 1.0f;
capriele 1:ba2d31e3112d 388 A[0][1] = -gx*dt*0.5f;
capriele 1:ba2d31e3112d 389 A[0][2] = -gy*dt*0.5f;
capriele 1:ba2d31e3112d 390 A[0][3] = -gz*dt*0.5f;
capriele 1:ba2d31e3112d 391 A[1][0] = gx*dt*0.5f;
capriele 1:ba2d31e3112d 392 A[1][1] = 1.0f;
capriele 1:ba2d31e3112d 393 A[1][2] = gz*dt*0.5f;
capriele 1:ba2d31e3112d 394 A[1][3] = -gy*dt*0.5f;
capriele 1:ba2d31e3112d 395 A[2][0] = gy*dt*0.5f;
capriele 1:ba2d31e3112d 396 A[2][1] = -gz*dt*0.5f;
capriele 1:ba2d31e3112d 397 A[2][2] = 1.0f;
capriele 1:ba2d31e3112d 398 A[2][3] = gx*dt*0.5f;
capriele 1:ba2d31e3112d 399 A[3][0] = gz*dt*0.5f;
capriele 1:ba2d31e3112d 400 A[3][1] = gy*dt*0.5f;
capriele 1:ba2d31e3112d 401 A[3][2] = -gx*dt*0.5f;
capriele 1:ba2d31e3112d 402 A[3][3] = 1.0f;
capriele 1:ba2d31e3112d 403
capriele 1:ba2d31e3112d 404 // Aggiorno la Covarianza
capriele 1:ba2d31e3112d 405 float P0[STATE_SIZE][STATE_SIZE];
capriele 1:ba2d31e3112d 406 arm_matrix_instance_f32 P0m = {STATE_SIZE, STATE_SIZE, (float *)P0};
capriele 1:ba2d31e3112d 407
capriele 1:ba2d31e3112d 408 mat_mult(&Am, &Pm, &tmpNN1m); // A P
capriele 1:ba2d31e3112d 409 mat_trans(&Am, &tmpNN2m); // A'
capriele 1:ba2d31e3112d 410 mat_mult(&tmpNN1m, &tmpNN2m, &P0m); // A P A'
capriele 1:ba2d31e3112d 411 for(int i = 0; i < STATE_SIZE; ++i){
capriele 1:ba2d31e3112d 412 P0[i][i] += Q_VARIANCE;
capriele 1:ba2d31e3112d 413 }
capriele 1:ba2d31e3112d 414
capriele 1:ba2d31e3112d 415 // Calcolo il guadagno
capriele 1:ba2d31e3112d 416 float K[STATE_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 417 arm_matrix_instance_f32 Km = {STATE_SIZE, OUTPUT_SIZE, (float *)K};
capriele 1:ba2d31e3112d 418
capriele 1:ba2d31e3112d 419 float h[OUTPUT_SIZE][STATE_SIZE];
capriele 1:ba2d31e3112d 420 arm_matrix_instance_f32 Hm = {OUTPUT_SIZE, STATE_SIZE, (float *)h};
capriele 1:ba2d31e3112d 421
capriele 1:ba2d31e3112d 422 float hT[STATE_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 423 arm_matrix_instance_f32 HTm = {STATE_SIZE, OUTPUT_SIZE, (float *)hT};
capriele 1:ba2d31e3112d 424
capriele 1:ba2d31e3112d 425 float P0hT[STATE_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 426 arm_matrix_instance_f32 P0HTm = {STATE_SIZE, OUTPUT_SIZE, (float *)P0hT};
capriele 1:ba2d31e3112d 427
capriele 1:ba2d31e3112d 428 float hP0hT[OUTPUT_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 429 arm_matrix_instance_f32 HP0HTm = {OUTPUT_SIZE, OUTPUT_SIZE, (float *)hP0hT};
capriele 1:ba2d31e3112d 430
capriele 1:ba2d31e3112d 431 float hP0hT_inv[OUTPUT_SIZE][OUTPUT_SIZE];
capriele 1:ba2d31e3112d 432 arm_matrix_instance_f32 HP0HT_INVm = {OUTPUT_SIZE, OUTPUT_SIZE, (float *)hP0hT_inv};
capriele 1:ba2d31e3112d 433
capriele 1:ba2d31e3112d 434 //Accelerometer
capriele 1:ba2d31e3112d 435 h[0][0] = -2.0f*q2;
capriele 1:ba2d31e3112d 436 h[0][1] = 2.0f*q3;
capriele 1:ba2d31e3112d 437 h[0][2] = -2.0f*q0;
capriele 1:ba2d31e3112d 438 h[0][3] = 2.0f*q1;
capriele 1:ba2d31e3112d 439 h[1][0] = 2.0f*q1;
capriele 1:ba2d31e3112d 440 h[1][1] = 2.0f*q0;
capriele 1:ba2d31e3112d 441 h[1][2] = 2.0f*q3;
capriele 1:ba2d31e3112d 442 h[1][3] = 2.0f*q2;
capriele 1:ba2d31e3112d 443 h[2][0] = 4.0f*q0;
capriele 1:ba2d31e3112d 444 h[2][1] = 0.0f;
capriele 1:ba2d31e3112d 445 h[2][2] = 0.0f;
capriele 1:ba2d31e3112d 446 h[2][3] = 4.0f*q3;
capriele 1:ba2d31e3112d 447 //h[2][0] = 2.0f*q0;
capriele 1:ba2d31e3112d 448 //h[2][1] = -2.0f*q1;
capriele 1:ba2d31e3112d 449 //h[2][2] = -2.0f*q2;
capriele 1:ba2d31e3112d 450 //h[2][3] = 2.0f*q3;
capriele 1:ba2d31e3112d 451
capriele 1:ba2d31e3112d 452 //Magnetometer
capriele 1:ba2d31e3112d 453 h[3][0] = 4.0f*b2*q0 - 2.0f*b4*q2;
capriele 1:ba2d31e3112d 454 h[3][1] = 4.0f*b2*q1 + 2.0f*b4*q3;
capriele 1:ba2d31e3112d 455 h[3][2] = -2.0f*b4*q0;
capriele 1:ba2d31e3112d 456 h[3][3] = 2.0f*b4*q1;
capriele 1:ba2d31e3112d 457 h[4][0] = 2.0f*b4*q1 - 2.0f*b2*q3;
capriele 1:ba2d31e3112d 458 h[4][1] = 2.0f*b2*q2 + 2.0f*b4*q0;
capriele 1:ba2d31e3112d 459 h[4][2] = 2.0f*b2*q1 + 2.0f*b4*q3;
capriele 1:ba2d31e3112d 460 h[4][3] = 2.0f*b4*q2 - 2.0f*b2*q0;
capriele 1:ba2d31e3112d 461 h[5][0] = 2.0f*b2*q2 + 4.0f*b4*q0;
capriele 1:ba2d31e3112d 462 h[5][1] = 2.0f*b2*q3;
capriele 1:ba2d31e3112d 463 h[5][2] = 2.0f*b2*q0;
capriele 1:ba2d31e3112d 464 h[5][3] = 2.0f*b2*q1 + 4.0f*b4*q3;
capriele 1:ba2d31e3112d 465
capriele 1:ba2d31e3112d 466 //h[3][0] = -2.0f*b4*q2;
capriele 1:ba2d31e3112d 467 //h[3][1] = 2.0f*b4*q3;
capriele 1:ba2d31e3112d 468 //h[3][2] = -4.0f*b2*q2 - 2.0f*b4*q0;
capriele 1:ba2d31e3112d 469 //h[3][3] = -4.0f*b2*q3 + 2.0f*b4*q1;
capriele 1:ba2d31e3112d 470 //h[4][0] = -2.0f*b2*q3 + 2.0f*b4*q1;
capriele 1:ba2d31e3112d 471 //h[4][1] = 2.0f*b2*q2 + 2.0f*b4*q0;
capriele 1:ba2d31e3112d 472 //h[4][2] = 2.0f*b2*q1 + 2.0f*b4*q3;
capriele 1:ba2d31e3112d 473 //h[4][3] = -2.0f*b2*q0 + 2.0f*b4*q2;
capriele 1:ba2d31e3112d 474 //h[5][0] = 2.0f*b2*q2;
capriele 1:ba2d31e3112d 475 //h[5][1] = 2.0f*b2*q3 - 4.0f*b4*q1;
capriele 1:ba2d31e3112d 476 //h[5][2] = 2.0f*b2*q0 - 4.0f*b4*q2;
capriele 1:ba2d31e3112d 477 //h[5][3] = 2.0f*b2*q1;
capriele 1:ba2d31e3112d 478
capriele 1:ba2d31e3112d 479
capriele 1:ba2d31e3112d 480 // ====== INNOVATION COVARIANCE ======
capriele 1:ba2d31e3112d 481 mat_trans(&Hm, &HTm);
capriele 1:ba2d31e3112d 482 mat_mult(&P0m, &HTm, &P0HTm); // P0*H'
capriele 1:ba2d31e3112d 483 mat_mult(&Hm, &P0HTm, &HP0HTm); // H*P0*H'
capriele 1:ba2d31e3112d 484 for (int i = 0; i < OUTPUT_SIZE; ++i) { // Add the element of HPH' to the above
capriele 1:ba2d31e3112d 485 hP0hT[i][i] += R[i];
capriele 1:ba2d31e3112d 486 }
capriele 1:ba2d31e3112d 487 mat_inv(&HP0HTm, &HP0HT_INVm); // (H*P0*H' + R)^(-1)
capriele 1:ba2d31e3112d 488 mat_mult(&P0HTm, &HP0HT_INVm, &Km); // K = P0*H'*(H*P0*H' + R)^(-1)
capriele 1:ba2d31e3112d 489
capriele 1:ba2d31e3112d 490 //Aggiorno Stato
capriele 1:ba2d31e3112d 491 float State[STATE_SIZE][1];
capriele 1:ba2d31e3112d 492 arm_matrix_instance_f32 Statem = {STATE_SIZE, 1, (float *)State};
capriele 1:ba2d31e3112d 493 float StateTMP[STATE_SIZE][1];
capriele 1:ba2d31e3112d 494 arm_matrix_instance_f32 StateTMPm = {STATE_SIZE, 1, (float *)StateTMP};
capriele 1:ba2d31e3112d 495 float Error[OUTPUT_SIZE][1];
capriele 1:ba2d31e3112d 496 arm_matrix_instance_f32 Errorm = {STATE_SIZE, 1, (float *)Error};
capriele 1:ba2d31e3112d 497 float ErrorTMP[OUTPUT_SIZE][1];
capriele 1:ba2d31e3112d 498 arm_matrix_instance_f32 ErrorTMPm = {STATE_SIZE, 1, (float *)ErrorTMP};
capriele 1:ba2d31e3112d 499 State[0][0] = q0;
capriele 1:ba2d31e3112d 500 State[1][0] = q1;
capriele 1:ba2d31e3112d 501 State[2][0] = q2;
capriele 1:ba2d31e3112d 502 State[3][0] = q3;
capriele 1:ba2d31e3112d 503 mat_mult(&Am, &Statem, &StateTMPm); // A*x(k)
capriele 1:ba2d31e3112d 504
capriele 1:ba2d31e3112d 505 Error[0][0] = ax - 2.0f*(q1*q3 - q0*q2);
capriele 1:ba2d31e3112d 506 Error[1][0] = ay - 2.0f*(q0*q1 + q2*q3);
capriele 1:ba2d31e3112d 507 Error[2][0] = az - 2.0f*(q0*q0 + q3*q3 - 0.5f);
capriele 1:ba2d31e3112d 508 Error[3][0] = mx - 2.0f*(b2*(q0*q0 + q1*q1 - 0.5f) - b4*(q0*q2 - q1*q3));
capriele 1:ba2d31e3112d 509 Error[4][0] = my - 2.0f*(b4*(q0*q1 + q2*q3) - b2*(q0*q3 - q1*q2));
capriele 1:ba2d31e3112d 510 Error[5][0] = mz - 2.0f*(b4*(q0*q0 + q3*q3 - 0.5f) + b2*(q0*q2 + q1*q3));
capriele 1:ba2d31e3112d 511 mat_mult(&Km, &Errorm, &ErrorTMPm); // K*error(k)
capriele 1:ba2d31e3112d 512
capriele 1:ba2d31e3112d 513 q0 = StateTMP[0][0] + ErrorTMP[0][0];
capriele 1:ba2d31e3112d 514 q1 = StateTMP[1][0] + ErrorTMP[1][0];
capriele 1:ba2d31e3112d 515 q2 = StateTMP[2][0] + ErrorTMP[2][0];
capriele 1:ba2d31e3112d 516 q3 = StateTMP[3][0] + ErrorTMP[3][0];
capriele 1:ba2d31e3112d 517
capriele 1:ba2d31e3112d 518 //Aggiorno la P
capriele 1:ba2d31e3112d 519 mat_mult(&Km, &Hm, &tmpNN1m); // KH
capriele 1:ba2d31e3112d 520 for (int i = 0; i < STATE_SIZE; ++i) {
capriele 1:ba2d31e3112d 521 tmpNN1d[i][i] -= 1.0f;
capriele 1:ba2d31e3112d 522 for (int j = 0; j < STATE_SIZE; ++j) {
capriele 1:ba2d31e3112d 523 tmpNN1d[i][j] *= -1.0f;
capriele 1:ba2d31e3112d 524 }
capriele 1:ba2d31e3112d 525 } // -(KH - I)
capriele 1:ba2d31e3112d 526 mat_mult(&tmpNN1m, &P0m, &Pm); // -(KH - I)*P0
capriele 1:ba2d31e3112d 527
capriele 1:ba2d31e3112d 528 // Normalise quaternion
capriele 1:ba2d31e3112d 529 recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
capriele 1:ba2d31e3112d 530 q0 *= recipNorm;
capriele 1:ba2d31e3112d 531 q1 *= recipNorm;
capriele 1:ba2d31e3112d 532 q2 *= recipNorm;
capriele 1:ba2d31e3112d 533 q3 *= recipNorm;
capriele 1:ba2d31e3112d 534 }
capriele 1:ba2d31e3112d 535
capriele 1:ba2d31e3112d 536 void sensfusion9GetEulerRPY(float* roll, float* pitch, float* yaw)
capriele 1:ba2d31e3112d 537 {
capriele 1:ba2d31e3112d 538 float gx = gravX;
capriele 1:ba2d31e3112d 539 float gy = gravY;
capriele 1:ba2d31e3112d 540 float gz = gravZ;
capriele 1:ba2d31e3112d 541
capriele 1:ba2d31e3112d 542 if (gx > 1) gx = 1;
capriele 1:ba2d31e3112d 543 if (gx < -1) gx = -1;
capriele 1:ba2d31e3112d 544
capriele 1:ba2d31e3112d 545 *yaw = atan2f(2.0f*(q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180.0f / M_PI_F;
capriele 1:ba2d31e3112d 546 *pitch = asinf(gx) * 180.0f / M_PI_F; //Pitch seems to be inverted
capriele 1:ba2d31e3112d 547 *roll = atan2f(gy, gz) * 180.0f / M_PI_F;
capriele 1:ba2d31e3112d 548 }
capriele 1:ba2d31e3112d 549 void sensfusion9GetQuaternion(float* Q0, float* Q1, float* Q2, float* Q3)
capriele 1:ba2d31e3112d 550 {
capriele 1:ba2d31e3112d 551 *Q0 = q0;
capriele 1:ba2d31e3112d 552 *Q1 = q1;
capriele 1:ba2d31e3112d 553 *Q2 = q2;
capriele 1:ba2d31e3112d 554 *Q3 = q3;
capriele 1:ba2d31e3112d 555 }
capriele 1:ba2d31e3112d 556
capriele 1:ba2d31e3112d 557 float sensfusion9GetAccZWithoutGravity(const float ax, const float ay, const float az)
capriele 1:ba2d31e3112d 558 {
capriele 1:ba2d31e3112d 559 return sensfusion9GetAccZ(ax, ay, az) - baseZacc;
capriele 1:ba2d31e3112d 560 }
capriele 1:ba2d31e3112d 561
capriele 1:ba2d31e3112d 562 float sensfusion9GetInvThrustCompensationForTilt()
capriele 1:ba2d31e3112d 563 {
capriele 1:ba2d31e3112d 564 // Return the z component of the estimated gravity direction
capriele 1:ba2d31e3112d 565 // (0, 0, 1) dot G
capriele 1:ba2d31e3112d 566 return gravZ;
capriele 1:ba2d31e3112d 567 }
capriele 1:ba2d31e3112d 568
capriele 1:ba2d31e3112d 569 //---------------------------------------------------------------------------------------------------
capriele 1:ba2d31e3112d 570 // Fast inverse square-root
capriele 1:ba2d31e3112d 571 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
capriele 1:ba2d31e3112d 572 float invSqrt(float x)
capriele 1:ba2d31e3112d 573 {
capriele 1:ba2d31e3112d 574 float halfx = 0.5f * x;
capriele 1:ba2d31e3112d 575 float y = x;
capriele 1:ba2d31e3112d 576 long i = *(long*)&y;
capriele 1:ba2d31e3112d 577 i = 0x5f3759df - (i>>1);
capriele 1:ba2d31e3112d 578 y = *(float*)&i;
capriele 1:ba2d31e3112d 579 y = y * (1.5f - (halfx * y * y));
capriele 1:ba2d31e3112d 580 return y;
capriele 1:ba2d31e3112d 581 }
capriele 1:ba2d31e3112d 582
capriele 1:ba2d31e3112d 583 static float sensfusion9GetAccZ(const float ax, const float ay, const float az)
capriele 1:ba2d31e3112d 584 {
capriele 1:ba2d31e3112d 585 // return vertical acceleration
capriele 1:ba2d31e3112d 586 // (A dot G) / |G|, (|G| = 1) -> (A dot G)
capriele 1:ba2d31e3112d 587 return (ax * gravX + ay * gravY + az * gravZ);
capriele 1:ba2d31e3112d 588 }
capriele 1:ba2d31e3112d 589
capriele 1:ba2d31e3112d 590 static void estimatedGravityDirection(float* gx, float* gy, float* gz)
capriele 1:ba2d31e3112d 591 {
capriele 1:ba2d31e3112d 592 *gx = 2 * (q1 * q3 - q0 * q2);
capriele 1:ba2d31e3112d 593 *gy = 2 * (q0 * q1 + q2 * q3);
capriele 1:ba2d31e3112d 594 *gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
capriele 1:ba2d31e3112d 595 }