Import Freescale's Kalman filter

Makefile

@@ -35,7 +35,7 @@ VERSION = 0.01
 
 endif
 
-OBJS = visualize.o serialdata.o rawdata.o magcal.o matrix.o
+OBJS = visualize.o serialdata.o rawdata.o magcal.o matrix.o fusion.o
 
 all: $(ALL)
 
@@ -68,5 +68,6 @@ serialdata.o: serialdata.c imuread.h
 rawdata.o: rawdata.c imuread.h
 magcal.o: magcal.c imuread.h
 matrix.o: matrix.c imuread.h
+fusion.o: fusion.c imuread.h
 
 

imuread.h

@@ -39,7 +39,7 @@
 
 #define TIMEOUT_MSEC 33
 
-#define MAGBUFFSIZE 450 // Freescale's lib needs at least 392
+#define MAGBUFFSIZE 650 // Freescale's lib needs at least 392
 
 #ifdef __cplusplus
 extern "C"{
@@ -53,10 +53,10 @@ typedef struct {
 } Point_t;
 
 typedef struct {
-	float w;
-	float x;
-	float y;
-	float z;
+	float q0; // w
+	float q1; // x
+	float q2; // y
+	float q3; // z
 } Quaternion_t;
 extern Quaternion_t current_orientation;
 
@@ -108,6 +108,104 @@ void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivo
 void fmatrixAeqRenormRotA(float A[][3]);
 
 
+#define OVERSAMPLE_RATIO 1
+
+// accelerometer sensor structure definition
+typedef struct
+{
+	int32_t iSumGpFast[3];    // sum of fast measurements
+	float fGpFast[3];       // fast (typically 200Hz) readings (g)
+	float fGp[3];           // slow (typically 25Hz) averaged readings (g)
+	float fgPerCount;       // initialized to FGPERCOUNT
+	int16_t iGpFast[3];       // fast (typically 200Hz) readings
+	int16_t iGp[3];           // slow (typically 25Hz) averaged readings (counts)
+} AccelSensor_t;
+
+// magnetometer sensor structure definition
+typedef struct
+{
+	int32_t iSumBpFast[3];    // sum of fast measurements
+	float fBpFast[3];       // fast (typically 200Hz) raw readings (uT)
+	float fBp[3];           // slow (typically 25Hz) averaged raw readings (uT)
+	float fBcFast[3];       // fast (typically 200Hz) calibrated readings (uT)
+	float fBc[3];           // slow (typically 25Hz) averaged calibrated readings (uT)
+	float fuTPerCount;      // initialized to FUTPERCOUNT
+	float fCountsPeruT;     // initialized to FCOUNTSPERUT
+	int16_t iBpFast[3];       // fast (typically 200Hz) raw readings (counts)
+	int16_t iBp[3];           // slow (typically 25Hz) averaged raw readings (counts)
+	int16_t iBc[3];           // slow (typically 25Hz) averaged calibrated readings (counts)
+} MagSensor_t;
+
+// gyro sensor structure definition
+typedef struct
+{
+	int32_t iSumYpFast[3];                    // sum of fast measurements
+	float fYp[3];                           // raw gyro sensor output (deg/s)
+	float fDegPerSecPerCount;               // initialized to FDEGPERSECPERCOUNT
+	int16_t iYpFast[OVERSAMPLE_RATIO][3];     // fast (typically 200Hz) readings
+	int16_t iYp[3];                           // averaged gyro sensor output (counts)
+} 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 fPhiPl;			// roll (deg)
+	float fThePl;			// pitch (deg)
+	float fPsiPl;			// yaw (deg)
+	float fRhoPl;			// compass (deg)
+	float fChiPl;			// tilt from vertical (deg)
+	// orientation matrix, quaternion and rotation vector
+	float fRPl[3][3];		// a posteriori orientation matrix
+	Quaternion_t fqPl;		// a posteriori orientation quaternion
+	float fRVecPl[3];		// rotation vector
+	// angular velocity
+	float fOmega[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 fbPl[3];			// gyro offset (deg/s)
+	float fThErrPl[3];		// orientation error (deg)
+	float fbErrPl[3];		// gyro offset error (deg/s)
+	// end elements transmitted in kalman packet
+
+	float fdErrGlPl[3];		// magnetic disturbance error (uT, global frame)
+	float fdErrSePl[3];		// magnetic disturbance error (uT, sensor frame)
+	float faErrSePl[3];		// linear acceleration error (g, sensor frame)
+	float faSeMi[3];		// linear acceleration (g, sensor frame)
+	float fDeltaPl;			// inclination angle (deg)
+	float faSePl[3];		// linear acceleration (g, sensor frame)
+	float faGlPl[3];		// linear acceleration (g, global frame)
+	float fgErrSeMi[3];		// difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro)
+	float fmErrSeMi[3];		// difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro)
+	float fgSeGyMi[3];		// gravity vector (g, sensor frame) measurement from gyro
+	float fmSeGyMi[3];		// geomagnetic vector (uT, sensor frame) measurement from gyro
+	float fmGl[3];			// geomagnetic vector (uT, global frame)
+	float fQvAA;			// accelerometer terms of Qv
+	float fQvMM;			// magnetometer terms of Qv
+	float fPPlus12x12[12][12];	// covariance matrix P+
+	float fK12x6[12][6];		// kalman filter gain matrix K
+	float fQw12x12[12][12];		// covariance matrix Qw
+	float fC6x12[6][12];		// measurement matrix C
+	float fRMi[3][3];		// a priori orientation matrix
+	Quaternion_t fDeltaq;		// delta quaternion
+	Quaternion_t fqMi;		// a priori orientation quaternion
+	float fcasq;			// FCA * FCA;
+	float fcdsq;			// FCD * FCD;
+	float fFastdeltat;		// sensor sampling interval (s) = 1 / SENSORFS
+	float fdeltat;			// kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
+	float fdeltatsq;		// fdeltat * fdeltat
+	float fQwbplusQvG;		// FQWB + FQVG
+	int16_t iFirstOrientationLock;	// denotes that 9DOF orientation has locked to 6DOF
+	int8_t resetflag;		// flag to request re-initialization on next pass
+} SV_9DOF_GBY_KALMAN_t;
+
+
 
 #ifdef __cplusplus
 } // extern "C"

serialdata.c

@@ -17,10 +17,10 @@ static int packet_primary_data(const unsigned char *data)
 	//current_position.x = (float)((int16_t)((data[13] << 8) | data[12])) / 10.0f;
 	//current_position.y = (float)((int16_t)((data[15] << 8) | data[14])) / 10.0f;
 	//current_position.z = (float)((int16_t)((data[17] << 8) | data[16])) / 10.0f;
-	current_orientation.w = (float)((int16_t)((data[25] << 8) | data[24])) / 30000.0f;
-	current_orientation.x = (float)((int16_t)((data[27] << 8) | data[26])) / 30000.0f;
-	current_orientation.y = (float)((int16_t)((data[29] << 8) | data[28])) / 30000.0f;
-	current_orientation.z = (float)((int16_t)((data[31] << 8) | data[30])) / 30000.0f;
+	current_orientation.q0 = (float)((int16_t)((data[25] << 8) | data[24])) / 30000.0f;
+	current_orientation.q1 = (float)((int16_t)((data[27] << 8) | data[26])) / 30000.0f;
+	current_orientation.q2 = (float)((int16_t)((data[29] << 8) | data[28])) / 30000.0f;
+	current_orientation.q3 = (float)((int16_t)((data[31] << 8) | data[30])) / 30000.0f;
 #if 0
 	printf("mag data, %5.2f %5.2f %5.2f\n",
 		current_position.x,

visualize.c

@@ -61,11 +61,10 @@ static void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t
 
 static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix)
 {
-	float qx = quat->x;
-	float qy = quat->y;
-	float qz = quat->z;
-	float qw = quat->w;
-
+	float qw = quat->q0;
+	float qx = quat->q1;
+	float qy = quat->q2;
+	float qz = quat->q3;
 	rmatrix[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz;
 	rmatrix[1] = 2.0f * qx * qy - 2.0f * qz * qw;
 	rmatrix[2] = 2.0f * qx * qz + 2.0f * qy * qw;