From 5f75764ee6d3e3349de0a50ee1a7d5e63b37ddaf Mon Sep 17 00:00:00 2001 From: PaulStoffregen Date: Sun, 20 Mar 2016 12:56:38 -0700 Subject: [PATCH] Consolidate fusion algorithm to single file --- fusion.c | 84 +++++++++++++++++++++++++++++++++++++++++++++++++++++-- imuread.h | 73 +++++------------------------------------------ rawdata.c | 18 ++++++------ 3 files changed, 97 insertions(+), 78 deletions(-) diff --git a/fusion.c b/fusion.c index 15cd8f5..c85151c 100644 --- a/fusion.c +++ b/fusion.c @@ -80,6 +80,87 @@ static float fatan_deg(float x); static float fatan2_deg(float y, float x); static float fatan_15deg(float x); +// 9DOF Kalman filter accelerometer, magnetometer and gyroscope state vector structure +typedef struct +{ + // start: elements common to all motion state vectors + // Euler angles + float PhiPl; // roll (deg) + float ThePl; // pitch (deg) + float PsiPl; // yaw (deg) + float RhoPl; // compass (deg) + float ChiPl; // tilt from vertical (deg) + // orientation matrix, quaternion and rotation vector + float RPl[3][3]; // a posteriori orientation matrix + Quaternion_t qPl; // a posteriori orientation quaternion + float RVecPl[3]; // rotation vector + // angular velocity + float Omega[3]; // angular velocity (deg/s) + // systick timer for benchmarking + int32_t systick; // systick timer; + // end: elements common to all motion state vectors + + // elements transmitted over bluetooth in kalman packet + float bPl[3]; // gyro offset (deg/s) + float ThErrPl[3]; // orientation error (deg) + float bErrPl[3]; // gyro offset error (deg/s) + // end elements transmitted in kalman packet + + float dErrGlPl[3]; // magnetic disturbance error (uT, global frame) + float dErrSePl[3]; // magnetic disturbance error (uT, sensor frame) + float aErrSePl[3]; // linear acceleration error (g, sensor frame) + float aSeMi[3]; // linear acceleration (g, sensor frame) + float DeltaPl; // inclination angle (deg) + float aSePl[3]; // linear acceleration (g, sensor frame) + float aGlPl[3]; // linear acceleration (g, global frame) + float gErrSeMi[3]; // difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro) + float mErrSeMi[3]; // difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro) + float gSeGyMi[3]; // gravity vector (g, sensor frame) measurement from gyro + float mSeGyMi[3]; // geomagnetic vector (uT, sensor frame) measurement from gyro + float mGl[3]; // geomagnetic vector (uT, global frame) + float QvAA; // accelerometer terms of Qv + float QvMM; // magnetometer terms of Qv + float PPlus12x12[12][12]; // covariance matrix P+ + float K12x6[12][6]; // kalman filter gain matrix K + float Qw12x12[12][12]; // covariance matrix Qw + float C6x12[6][12]; // measurement matrix C + float RMi[3][3]; // a priori orientation matrix + Quaternion_t Deltaq; // delta quaternion + Quaternion_t qMi; // a priori orientation quaternion + float casq; // FCA * FCA; + float cdsq; // FCD * FCD; + float Fastdeltat; // sensor sampling interval (s) = 1 / SENSORFS + float deltat; // kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS + float deltatsq; // fdeltat * fdeltat + float QwbplusQvG; // FQWB + FQVG + int16_t FirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF + int8_t resetflag; // flag to request re-initialization on next pass +} SV_9DOF_GBY_KALMAN_t; + + +SV_9DOF_GBY_KALMAN_t fusionstate; + +void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV); +void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, + const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro, + const MagCalibration_t *MagCal); + + +void fusion_init(void) +{ + fInit_9DOF_GBY_KALMAN(&fusionstate); +} + +void fusion_update(const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro, + const MagCalibration_t *MagCal) +{ + fRun_9DOF_GBY_KALMAN(&fusionstate, Accel, Mag, Gyro, MagCal); +} + +void fusion_read(Quaternion_t *q) +{ + memcpy(q, &(fusionstate.qPl), sizeof(Quaternion_t)); +} // function initializes the 9DOF Kalman filter void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV) @@ -227,8 +308,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, for (j = 0; j < OVERSAMPLE_RATIO; j++) { // compute the incremental fast (typically 200Hz) rotation vector rvec (deg) for (i = X; i <= Z; i++) { - rvec[i] = (((float)Gyro->YpFast[j][i] * DEG_PER_SEC_PER_COUNT) - SV->bPl[i]) - * SV->Fastdeltat; + rvec[i] = (Gyro->YpFast[j][i] - SV->bPl[i]) * SV->Fastdeltat; } // compute the incremental quaternion fDeltaq from the rotation vector diff --git a/imuread.h b/imuread.h index 9064d3f..adafef5 100644 --- a/imuread.h +++ b/imuread.h @@ -128,16 +128,16 @@ void fmatrixAeqRenormRotA(float A[][3]); #define G_PER_COUNT 0.0001220703125F // = 1/8192 typedef struct { - float GpFast[3]; // fast (typically 200Hz) readings (g) float Gp[3]; // slow (typically 25Hz) averaged readings (g) + float GpFast[3]; // fast (typically 200Hz) readings (g) } AccelSensor_t; // magnetometer sensor structure definition #define UT_PER_COUNT 0.1F typedef struct { - float BcFast[3]; // fast (typically 200Hz) calibrated readings (uT) float Bc[3]; // slow (typically 25Hz) averaged calibrated readings (uT) + float BcFast[3]; // fast (typically 200Hz) calibrated readings (uT) } MagSensor_t; // gyro sensor structure definition @@ -145,74 +145,15 @@ typedef struct typedef struct { float Yp[3]; // raw gyro sensor output (deg/s) - int16_t YpFast[OVERSAMPLE_RATIO][3]; // fast (typically 200Hz) readings + float YpFast[OVERSAMPLE_RATIO][3]; // fast (typically 200Hz) readings } GyroSensor_t; -// 9DOF Kalman filter accelerometer, magnetometer and gyroscope state vector structure -typedef struct -{ - // start: elements common to all motion state vectors - // Euler angles - float PhiPl; // roll (deg) - float ThePl; // pitch (deg) - float PsiPl; // yaw (deg) - float RhoPl; // compass (deg) - float ChiPl; // tilt from vertical (deg) - // orientation matrix, quaternion and rotation vector - float RPl[3][3]; // a posteriori orientation matrix - Quaternion_t qPl; // a posteriori orientation quaternion - float RVecPl[3]; // rotation vector - // angular velocity - float Omega[3]; // angular velocity (deg/s) - // systick timer for benchmarking - int32_t systick; // systick timer; - // end: elements common to all motion state vectors - - // elements transmitted over bluetooth in kalman packet - float bPl[3]; // gyro offset (deg/s) - float ThErrPl[3]; // orientation error (deg) - float bErrPl[3]; // gyro offset error (deg/s) - // end elements transmitted in kalman packet - - float dErrGlPl[3]; // magnetic disturbance error (uT, global frame) - float dErrSePl[3]; // magnetic disturbance error (uT, sensor frame) - float aErrSePl[3]; // linear acceleration error (g, sensor frame) - float aSeMi[3]; // linear acceleration (g, sensor frame) - float DeltaPl; // inclination angle (deg) - float aSePl[3]; // linear acceleration (g, sensor frame) - float aGlPl[3]; // linear acceleration (g, global frame) - float gErrSeMi[3]; // difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro) - float mErrSeMi[3]; // difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro) - float gSeGyMi[3]; // gravity vector (g, sensor frame) measurement from gyro - float mSeGyMi[3]; // geomagnetic vector (uT, sensor frame) measurement from gyro - float mGl[3]; // geomagnetic vector (uT, global frame) - float QvAA; // accelerometer terms of Qv - float QvMM; // magnetometer terms of Qv - float PPlus12x12[12][12]; // covariance matrix P+ - float K12x6[12][6]; // kalman filter gain matrix K - float Qw12x12[12][12]; // covariance matrix Qw - float C6x12[6][12]; // measurement matrix C - float RMi[3][3]; // a priori orientation matrix - Quaternion_t Deltaq; // delta quaternion - Quaternion_t qMi; // a priori orientation quaternion - float casq; // FCA * FCA; - float cdsq; // FCD * FCD; - float Fastdeltat; // sensor sampling interval (s) = 1 / SENSORFS - float deltat; // kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS - float deltatsq; // fdeltat * fdeltat - float QwbplusQvG; // FQWB + FQVG - int16_t FirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF - int8_t resetflag; // flag to request re-initialization on next pass -} SV_9DOF_GBY_KALMAN_t; - -void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV); -void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, - const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro, - const MagCalibration_t *MagCal); - - +void fusion_init(void); +void fusion_update(const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro, + const MagCalibration_t *MagCal); +void fusion_read(Quaternion_t *q); #ifdef __cplusplus } // extern "C" diff --git a/rawdata.c b/rawdata.c index 0925a33..1105f8a 100644 --- a/rawdata.c +++ b/rawdata.c @@ -5,12 +5,11 @@ static int rawcount=OVERSAMPLE_RATIO; static AccelSensor_t accel; static MagSensor_t mag; static GyroSensor_t gyro; -SV_9DOF_GBY_KALMAN_t fusionstate; void raw_data_reset(void) { rawcount = OVERSAMPLE_RATIO; - fInit_9DOF_GBY_KALMAN(&fusionstate); + fusion_init(); memset(&magcal, 0, sizeof(magcal)); magcal.V[2] = 80.0f; // initial guess magcal.invW[0][0] = 1.0f; @@ -75,7 +74,7 @@ void raw_data(const int16_t *data) magdiff = sqrtf(x * x + y * y + z * z); //printf("magdiff = %.2f\n", magdiff); if (magdiff > 0.8f) { - fInit_9DOF_GBY_KALMAN(&fusionstate); + fusion_init(); rawcount = OVERSAMPLE_RATIO; force_orientation_counter = 240; } @@ -84,7 +83,7 @@ void raw_data(const int16_t *data) if (force_orientation_counter > 0) { if (--force_orientation_counter == 0) { //printf("delayed forcible orientation reset\n"); - fInit_9DOF_GBY_KALMAN(&fusionstate); + fusion_init(); rawcount = OVERSAMPLE_RATIO; } } @@ -111,9 +110,9 @@ void raw_data(const int16_t *data) gyro.Yp[0] += x; gyro.Yp[1] += y; gyro.Yp[2] += z; - gyro.YpFast[rawcount][0] = data[3]; - gyro.YpFast[rawcount][1] = data[4]; - gyro.YpFast[rawcount][2] = data[5]; + gyro.YpFast[rawcount][0] = x; + gyro.YpFast[rawcount][1] = y; + gyro.YpFast[rawcount][2] = z; apply_calibration(data[6], data[7], data[8], &point); mag.BcFast[0] = point.x; @@ -135,9 +134,8 @@ void raw_data(const int16_t *data) mag.Bc[0] *= ratio; mag.Bc[1] *= ratio; mag.Bc[2] *= ratio; - fRun_9DOF_GBY_KALMAN(&fusionstate, &accel, &mag, &gyro, &magcal); - - memcpy(¤t_orientation, &(fusionstate.qPl), sizeof(Quaternion_t)); + fusion_update(&accel, &mag, &gyro, &magcal); + fusion_read(¤t_orientation); } }