Consolidate fusion algorithm to single file
This commit is contained in:
PaulStoffregen
parent
1d9800aeec
commit
5f75764ee6
3 changed files with 97 additions and 78 deletions
84
fusion.c
84
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
|
||||
|
|
|
|||
73
imuread.h
73
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"
|
||||
|
|
|
|||
18
rawdata.c
18
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue