Skip to content

Commit 85f6d7d

Browse files
pronenewbitspronenewbits
authored
Add files via upload
1 parent e875984 commit 85f6d7d

File tree

5 files changed

+37
-111
lines changed

5 files changed

+37
-111
lines changed

‎Sensor_Fusion_RLS_StateMachine.png

-50.3 KB
Loading

‎ahrs_ekf/ahrs_ekf.ino

Lines changed: 15 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -14,15 +14,15 @@ Matrix RLS_P(4,4); /* Inverse of correction estimation */
1414
Matrix RLS_in(4,1); /* Input data */
1515
Matrix RLS_out(1,1); /* Output data */
1616
Matrix RLS_gain(4,1); /* RLS gain */
17-
uint32_t RLS_u32iterData = 0; /* To track how much data we take */
17+
uint32_t RLS_u32iterData = 0; /* To track how much data we take */
1818

1919

2020
/* ================================================= EKF Variables/function declaration ================================================= */
2121
/* EKF initialization constant */
2222
#define P_INIT (10.)
2323
#define Q_INIT (1e-6)
24-
#define R_INIT_ACC (0.0015)
25-
#define R_INIT_MAG (0.0015)
24+
#define R_INIT_ACC (0.0015/10.)
25+
#define R_INIT_MAG (0.0015/10.)
2626
/* P(k=0) variable --------------------------------------------------------------------------------------------------------- */
2727
float_prec EKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0, 0, 0,
2828
0, P_INIT, 0, 0,
@@ -62,11 +62,11 @@ EKF EKF_IMU(quaternionData, EKF_PINIT, EKF_QINIT, EKF_RINIT,
6262
#define IMU_ACC_Z0 (1)
6363

6464
/* Magnetic vector constant (align with local magnetic vector) */
65-
float_prec IMU_MAG_B0_data[3] = {-0.239, -0.831, 0.502};
65+
float_prec IMU_MAG_B0_data[3] = {cos(0), sin(0), 0.000000};
6666
Matrix IMU_MAG_B0(3, 1, IMU_MAG_B0_data);
67-
67+
6868
/* The hard-magnet bias */
69-
float_prec HARD_IRON_BIAS_data[3] = {15.507348, 5.4278485, 13.934539};
69+
float_prec HARD_IRON_BIAS_data[3] = {8.832973, 7.243323, 23.95714};
7070
Matrix HARD_IRON_BIAS(3, 1, HARD_IRON_BIAS_data);
7171

7272
/* An MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68 */
@@ -143,7 +143,7 @@ void loop() {
143143
float p = IMU.getGyroX_rads();
144144
float q = IMU.getGyroY_rads();
145145
float r = IMU.getGyroZ_rads();
146-
146+
147147
if (STATE_AHRS == STATE_EKF_RUNNING) { /* Run the EKF algorithm */
148148

149149
/* ================== Read the sensor data / simulate the system here ================== */
@@ -326,9 +326,9 @@ void loop() {
326326

327327
/* Normalizing the acceleration vector & projecting the gravitational vector (gravity is negative acceleration) */
328328
float_prec _normG = sqrt((Ax * Ax) + (Ay * Ay) + (Az * Az));
329-
Ax = -Ax / _normG;
330-
Ay = -Ay / _normG;
331-
Az = -Az / _normG;
329+
Ax = Ax / _normG;
330+
Ay = Ay / _normG;
331+
Az = Az / _normG;
332332

333333
/* Normalizing the magnetic vector */
334334
_normG = sqrt((Bx * Bx) + (By * By) + (Bz * Bz));
@@ -341,11 +341,12 @@ void loop() {
341341
float roll = asin(Ay/cos(pitch));
342342
float m_tilt_x = Bx*cos(pitch) + By*sin(roll)*sin(pitch) + Bz*cos(roll)*sin(pitch);
343343
float m_tilt_y = + By*cos(roll) - Bz*sin(roll);
344-
float m_tilt_z = -Bx*sin(pitch) + By*sin(roll)*cos(pitch) + Bz*cos(roll)*cos(pitch);
344+
/* float m_tilt_z = -Bx*sin(pitch) + By*sin(roll)*cos(pitch) + Bz*cos(roll)*cos(pitch); */
345345

346-
IMU_MAG_B0[0][0] = m_tilt_x;
347-
IMU_MAG_B0[1][0] = m_tilt_y;
348-
IMU_MAG_B0[2][0] = m_tilt_z;
346+
float mag_dec = atan2(m_tilt_y, m_tilt_x);
347+
IMU_MAG_B0[0][0] = cos(mag_dec);
348+
IMU_MAG_B0[1][0] = sin(mag_dec);
349+
IMU_MAG_B0[2][0] = 0;
349350

350351
Serial.println("North identification finished, the north vector identified:");
351352
snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f\r\n", IMU_MAG_B0[0][0], IMU_MAG_B0[1][0], IMU_MAG_B0[2][0]);

‎ahrs_ekf/konfig.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,8 @@
1111
#include <stdint.h>
1212
#include <math.h>
1313

14+
#define PAKAI_MAGNET
15+
1416

1517
/* State Space dimension */
1618
#define SS_X_LEN (4)

‎ahrs_ekf/simple_mpu9250.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -196,14 +196,15 @@ class SimpleMPU9250
196196
_gx = ((float)(_gxcounts) * _gyroScale) - _gxb;
197197
_gy = ((float)(_gycounts) * _gyroScale) - _gyb;
198198
_gz = ((float)(_gzcounts) * _gyroScale) - _gzb;
199+
199200
/* Transform the magnetomer to align with the Accelerometer and Gyroscope Axis */
200201
float _hxAK8963 = ((float)_hxcounts) * _magScaleX;
201202
float _hyAK8963 = ((float)_hycounts) * _magScaleY;
202203
float _hzAK8963 = ((float)_hzcounts) * _magScaleZ;
203204
_hx = _hyAK8963;
204205
_hy = _hxAK8963;
205206
_hz = -_hzAK8963;
206-
207+
207208
return 1;
208209
}
209210

‎ahrs_ukf/ahrs_ukf.ino

Lines changed: 18 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -14,15 +14,15 @@ Matrix RLS_P(4,4); /* Inverse of correction estimation */
1414
Matrix RLS_in(4,1); /* Input data */
1515
Matrix RLS_out(1,1); /* Output data */
1616
Matrix RLS_gain(4,1); /* RLS gain */
17-
uint32_t RLS_u32iterData = 0; /* To track how much data we take */
17+
uint32_t RLS_u32iterData = 0; /* To track how much data we take */
1818

1919

2020
/* ================================================= UKF Variables/function declaration ================================================= */
2121
/* UKF initialization constant */
22-
#define P_INIT (10.)
23-
#define Rv_INIT (1e-6)
24-
#define Rn_INIT_ACC (0.0015)
25-
#define Rn_INIT_MAG (0.0015)
22+
#define P_INIT (10.)
23+
#define Rv_INIT (1e-6)
24+
#define Rn_INIT_ACC (0.0015/10.)
25+
#define Rn_INIT_MAG (0.0015/10.)
2626
/* P(k=0) variable --------------------------------------------------------------------------------------------------------- */
2727
float_prec UKF_PINIT_data[SS_X_LEN*SS_X_LEN] = {P_INIT, 0, 0, 0,
2828
0, P_INIT, 0, 0,
@@ -46,8 +46,6 @@ Matrix UKF_Rn(SS_Z_LEN, SS_Z_LEN, UKF_Rn_data);
4646
/* Nonlinear & linearization function -------------------------------------------------------------------------------------- */
4747
bool Main_bUpdateNonlinearX(Matrix &X_Next, Matrix &X, Matrix &U);
4848
bool Main_bUpdateNonlinearY(Matrix &Y, Matrix &X, Matrix &U);
49-
bool Main_bCalcJacobianF(Matrix &F, Matrix &X, Matrix &U);
50-
bool Main_bCalcJacobianH(Matrix &H, Matrix &X, Matrix &U);
5149
/* UKF variables ----------------------------------------------------------------------------------------------------------- */
5250
Matrix quaternionData(SS_X_LEN, 1);
5351
Matrix Y(SS_Z_LEN, 1);
@@ -61,11 +59,11 @@ UKF UKF_IMU(quaternionData, UKF_PINIT, UKF_Rv, UKF_Rn, Main_bUpdateNonlinearX, M
6159
#define IMU_ACC_Z0 (1)
6260

6361
/* Magnetic vector constant (align with local magnetic vector) */
64-
float_prec IMU_MAG_B0_data[3] = {-0.239, -0.831, 0.502};
62+
float_prec IMU_MAG_B0_data[3] = {cos(0), sin(0), 0.000000};
6563
Matrix IMU_MAG_B0(3, 1, IMU_MAG_B0_data);
66-
64+
6765
/* The hard-magnet bias */
68-
float_prec HARD_IRON_BIAS_data[3] = {15.507348, 5.4278485, 13.934539};
66+
float_prec HARD_IRON_BIAS_data[3] = {8.832973, 7.243323, 23.95714};
6967
Matrix HARD_IRON_BIAS(3, 1, HARD_IRON_BIAS_data);
7068

7169
/* An MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68 */
@@ -95,9 +93,9 @@ void setup() {
9593
/* Serial initialization -------------------------------------- */
9694
Serial.begin(115200);
9795
while(!Serial) {}
96+
Serial.println("Calibrating IMU bias...");
9897

9998
/* IMU initialization ----------------------------------------- */
100-
Serial.println("IMU initialization, calibrating gyro bias...");
10199
int status = IMU.begin(); /* start communication with IMU */
102100
if (status < 0) {
103101
Serial.println("IMU initialization unsuccessful");
@@ -142,7 +140,7 @@ void loop() {
142140
float p = IMU.getGyroX_rads();
143141
float q = IMU.getGyroY_rads();
144142
float r = IMU.getGyroZ_rads();
145-
143+
146144
if (STATE_AHRS == STATE_UKF_RUNNING) { /* Run the UKF algorithm */
147145

148146
/* ================== Read the sensor data / simulate the system here ================== */
@@ -325,9 +323,9 @@ void loop() {
325323

326324
/* Normalizing the acceleration vector & projecting the gravitational vector (gravity is negative acceleration) */
327325
float_prec _normG = sqrt((Ax * Ax) + (Ay * Ay) + (Az * Az));
328-
Ax = -Ax / _normG;
329-
Ay = -Ay / _normG;
330-
Az = -Az / _normG;
326+
Ax = Ax / _normG;
327+
Ay = Ay / _normG;
328+
Az = Az / _normG;
331329

332330
/* Normalizing the magnetic vector */
333331
_normG = sqrt((Bx * Bx) + (By * By) + (Bz * Bz));
@@ -340,11 +338,12 @@ void loop() {
340338
float roll = asin(Ay/cos(pitch));
341339
float m_tilt_x = Bx*cos(pitch) + By*sin(roll)*sin(pitch) + Bz*cos(roll)*sin(pitch);
342340
float m_tilt_y = + By*cos(roll) - Bz*sin(roll);
343-
float m_tilt_z = -Bx*sin(pitch) + By*sin(roll)*cos(pitch) + Bz*cos(roll)*cos(pitch);
341+
/* float m_tilt_z = -Bx*sin(pitch) + By*sin(roll)*cos(pitch) + Bz*cos(roll)*cos(pitch); */
344342

345-
IMU_MAG_B0[0][0] = m_tilt_x;
346-
IMU_MAG_B0[1][0] = m_tilt_y;
347-
IMU_MAG_B0[2][0] = m_tilt_z;
343+
float mag_dec = atan2(m_tilt_y, m_tilt_x);
344+
IMU_MAG_B0[0][0] = cos(mag_dec);
345+
IMU_MAG_B0[1][0] = sin(mag_dec);
346+
IMU_MAG_B0[2][0] = 0;
348347

349348
Serial.println("North identification finished, the north vector identified:");
350349
snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %.3f %.3f\r\n", IMU_MAG_B0[0][0], IMU_MAG_B0[1][0], IMU_MAG_B0[2][0]);
@@ -463,83 +462,6 @@ bool Main_bUpdateNonlinearY(Matrix &Y, Matrix &X, Matrix &U)
463462
return true;
464463
}
465464

466-
bool Main_bCalcJacobianF(Matrix &F, Matrix &X, Matrix &U)
467-
{
468-
/* In Main_bUpdateNonlinearX():
469-
* q0 = q0 + q0_dot * dT;
470-
* q1 = q1 + q1_dot * dT;
471-
* q2 = q2 + q2_dot * dT;
472-
* q3 = q3 + q3_dot * dT;
473-
*/
474-
float_prec p, q, r;
475-
476-
p = U[0][0];
477-
q = U[1][0];
478-
r = U[2][0];
479-
480-
F[0][0] = 1.000;
481-
F[1][0] = 0.5*p * SS_DT;
482-
F[2][0] = 0.5*q * SS_DT;
483-
F[3][0] = 0.5*r * SS_DT;
484-
485-
F[0][1] = -0.5*p * SS_DT;
486-
F[1][1] = 1.000;
487-
F[2][1] = -0.5*r * SS_DT;
488-
F[3][1] = 0.5*q * SS_DT;
489-
490-
F[0][2] = -0.5*q * SS_DT;
491-
F[1][2] = 0.5*r * SS_DT;
492-
F[2][2] = 1.000;
493-
F[3][2] = -0.5*p * SS_DT;
494-
495-
F[0][3] = -0.5*r * SS_DT;
496-
F[1][3] = -0.5*q * SS_DT;
497-
F[2][3] = 0.5*p * SS_DT;
498-
F[3][3] = 1.000;
499-
500-
return true;
501-
}
502-
503-
bool Main_bCalcJacobianH(Matrix &H, Matrix &X, Matrix &U)
504-
{
505-
float_prec q0, q1, q2, q3;
506-
507-
q0 = X[0][0];
508-
q1 = X[1][0];
509-
q2 = X[2][0];
510-
q3 = X[3][0];
511-
512-
H[0][0] = -2*q2 * IMU_ACC_Z0;
513-
H[1][0] = +2*q1 * IMU_ACC_Z0;
514-
H[2][0] = +2*q0 * IMU_ACC_Z0;
515-
H[3][0] = 2*q0*IMU_MAG_B0[0][0] + 2*q3*IMU_MAG_B0[1][0] - 2*q2*IMU_MAG_B0[2][0];
516-
H[4][0] = -2*q3*IMU_MAG_B0[0][0] + 2*q0*IMU_MAG_B0[1][0] + 2*q1*IMU_MAG_B0[2][0];
517-
H[5][0] = 2*q2*IMU_MAG_B0[0][0] - 2*q1*IMU_MAG_B0[1][0] + 2*q0*IMU_MAG_B0[2][0];
518-
519-
H[0][1] = +2*q3 * IMU_ACC_Z0;
520-
H[1][1] = +2*q0 * IMU_ACC_Z0;
521-
H[2][1] = -2*q1 * IMU_ACC_Z0;
522-
H[3][1] = 2*q1*IMU_MAG_B0[0][0]+2*q2*IMU_MAG_B0[1][0] + 2*q3*IMU_MAG_B0[2][0];
523-
H[4][1] = 2*q2*IMU_MAG_B0[0][0]-2*q1*IMU_MAG_B0[1][0] + 2*q0*IMU_MAG_B0[2][0];
524-
H[5][1] = 2*q3*IMU_MAG_B0[0][0]-2*q0*IMU_MAG_B0[1][0] - 2*q1*IMU_MAG_B0[2][0];
525-
526-
H[0][2] = -2*q0 * IMU_ACC_Z0;
527-
H[1][2] = +2*q3 * IMU_ACC_Z0;
528-
H[2][2] = -2*q2 * IMU_ACC_Z0;
529-
H[3][2] = -2*q2*IMU_MAG_B0[0][0]+2*q1*IMU_MAG_B0[1][0] - 2*q0*IMU_MAG_B0[2][0];
530-
H[4][2] = 2*q1*IMU_MAG_B0[0][0]+2*q2*IMU_MAG_B0[1][0] + 2*q3*IMU_MAG_B0[2][0];
531-
H[5][2] = 2*q0*IMU_MAG_B0[0][0]+2*q3*IMU_MAG_B0[1][0] - 2*q2*IMU_MAG_B0[2][0];
532-
533-
H[0][3] = +2*q1 * IMU_ACC_Z0;
534-
H[1][3] = +2*q2 * IMU_ACC_Z0;
535-
H[2][3] = +2*q3 * IMU_ACC_Z0;
536-
H[3][3] = -2*q3*IMU_MAG_B0[0][0]+2*q0*IMU_MAG_B0[1][0] + 2*q1*IMU_MAG_B0[2][0];
537-
H[4][3] = -2*q0*IMU_MAG_B0[0][0]-2*q3*IMU_MAG_B0[1][0] + 2*q2*IMU_MAG_B0[2][0];
538-
H[5][3] = 2*q1*IMU_MAG_B0[0][0]+2*q2*IMU_MAG_B0[1][0] + 2*q3*IMU_MAG_B0[2][0];
539-
540-
return true;
541-
}
542-
543465

544466

545467
void SPEW_THE_ERROR(char const * str)

0 commit comments

Comments
 (0)