jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Use Kalman filter, show orientation

Browse source
This commit is contained in:
PaulStoffregen 2016-03-12 12:00:24 -08:00
commit 9d108fedb3
5 changed files with 108 additions and 68 deletions
Download patch file Download diff file Expand all files Collapse all files

15
fusion.c
View file

@ -62,12 +62,12 @@
#define SENSORFS 100
#define OVERSAMPLE_RATIO 1
#define OVERSAMPLE_RATIO 4
static void fqAeq1(Quaternion_t *pqA);
static void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[]);
static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[]);
static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg,
float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg);
static void fQuaternionFromRotationMatrix(float R[][3], Quaternion_t *pq);
@ -159,8 +159,8 @@ void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, int16_t iSensorFS, int16_t
// 9DOF orientation function implemented using a 12 element Kalman filter
void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
AccelSensor_t *Accel, MagSensor_t *Mag, GyroSensor_t *Gyro,
MagCalibration_t *MagCal, int16_t iOverSampleRatio)
const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
const MagCalibration_t *MagCal, int16_t iOverSampleRatio)
{
// local scalars and arrays
float fopp, fadj, fhyp; // opposite, adjacent and hypoteneuse
@ -230,7 +230,8 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
for (j = 0; j < iOverSampleRatio; j++) {
// compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
for (i = X; i <= Z; i++) {
rvec[i] = (((float)Gyro->iYpFast[j][i] * Gyro->fDegPerSecPerCount) - SV->fbPl[i]) * SV->fFastdeltat;
rvec[i] = (((float)Gyro->iYpFast[j][i] * DEG_PER_SEC_PER_COUNT) - SV->fbPl[i])
* SV->fFastdeltat;
}
// compute the incremental quaternion fDeltaq from the rotation vector
@ -812,7 +813,7 @@ void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
// NED: 6DOF e-Compass function computing rotation matrix fR
static void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[])
static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[])
{
// local variables
float fmod[3]; // column moduli
@ -868,8 +869,6 @@ static void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[]
if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F))) {
*pfDelta = fasin_deg(fGdotBc / (fmod[Z] * fmodBc));
}
return;
}

6
imuread.c
View file

@ -23,11 +23,7 @@ static void glut_display_callback(void)
int main(int argc, char *argv[])
{
memset(&magcal, 0, sizeof(magcal));
magcal.fV[2] = 80.0f;
magcal.finvW[0][0] = 1.0f;
magcal.finvW[1][1] = 1.0f;
magcal.finvW[2][2] = 1.0f;
raw_data_reset();
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);

44
imuread.h
View file

@ -49,7 +49,7 @@ typedef struct {
float x;
float y;
float z;
int valid;
//int valid;
} Point_t;
typedef struct {
@ -63,8 +63,10 @@ extern Quaternion_t current_orientation;
extern int open_port(const char *name);
extern int read_serial_data(void);
extern void close_port(void);
void raw_data_reset(void);
void raw_data(const int16_t *data);
void visualize_init(void);
void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out);
void display_callback(void);
void resize_callback(int width, int height);
void MagCal_Run(void);
@ -108,42 +110,45 @@ void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivo
void fmatrixAeqRenormRotA(float A[][3]);
#define OVERSAMPLE_RATIO 1
#define OVERSAMPLE_RATIO 4
// accelerometer sensor structure definition
#define G_PER_COUNT 0.0001220703125F // = 1/8192
typedef struct
{
int32_t iSumGpFast[3]; // sum of fast measurements
//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)
//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
#define UT_PER_COUNT 0.1F
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)
//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)
//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
#define DEG_PER_SEC_PER_COUNT 0.0625F // = 1/16
typedef struct
{
int32_t iSumYpFast[3]; // sum of fast measurements
//int32_t iSumYpFast[3]; // sum of fast measurements
float fYp[3]; // raw gyro sensor output (deg/s)
float fDegPerSecPerCount; // initialized to FDEGPERSECPERCOUNT
//float fDegPerSecPerCount; // initialized to FDEGPERSECPERCOUNT
int16_t iYpFast[OVERSAMPLE_RATIO][3]; // fast (typically 200Hz) readings
int16_t iYp[3]; // averaged gyro sensor output (counts)
//int16_t iYp[3]; // averaged gyro sensor output (counts)
} GyroSensor_t;
@ -205,6 +210,11 @@ typedef struct
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, int16_t iSensorFS, int16_t iOverSampleRatio);
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, int16_t iOverSampleRatio);
#ifdef __cplusplus

96
rawdata.c
View file

@ -1,16 +1,22 @@
#include "imuread.h"
#define HIST_LEN 12
uint32_t mag_histogram[3][65536];
int raw_history[9][HIST_LEN];
int raw_history_count=0;
int raw_history_last=0;
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, 100, OVERSAMPLE_RATIO);
memset(&magcal, 0, sizeof(magcal));
magcal.fV[1] = 10.0f;
magcal.fV[2] = 80.0f; // initial guess
magcal.finvW[0][0] = 1.0f;
magcal.finvW[1][1] = 1.0f;
magcal.finvW[2][2] = 1.0f;
}
static void add_magcal_data(const int16_t *data)
@ -24,6 +30,8 @@ static void add_magcal_data(const int16_t *data)
if (!magcal.valid[i]) break;
}
// if no unused, find the ones closest to each other
// TODO: after reasonable sphere fit, we should retire older data
// and choose the ones farthest from the sphere's radius
if (i >= MAGBUFFSIZE) {
for (i=0; i < MAGBUFFSIZE; i++) {
for (j=i+1; j < MAGBUFFSIZE; j++) {
@ -51,30 +59,64 @@ static void add_magcal_data(const int16_t *data)
void raw_data(const int16_t *data)
{
//printf("raw_data: %d %d %d %d %d %d %d %d %d\n", data[0], data[1], data[2],
//data[3], data[4], data[5], data[6], data[7], data[8]);
mag_histogram[0][data[6]+32786]++;
mag_histogram[1][data[7]+32786]++;
mag_histogram[2][data[8]+32786]++;
// RdSensData_Run (tasks.c) - reads 8 samples, sums, scales to sci units
// Fusion_Run (tasks.c)
// fInvertMagCal - applies hard & soft iron cal
// fV[3] = hard iron -- ftrV[3]
// finvW[3][3] = soft iron
// iUpdateMagnetometerBuffer - adds data to buffer
// fRun_6DOF_GY_KALMAN
// fRun_9DOF_GBY_KALMAN
// MagCal_Run (tasks.c)
// fUpdateCalibration4INV
// fUpdateCalibration7EIG
// fUpdateCalibration10EIG
float x, y, z, ratio;
Point_t point;
add_magcal_data(data);
MagCal_Run();
if (rawcount >= OVERSAMPLE_RATIO) {
memset(&accel, 0, sizeof(accel));
memset(&mag, 0, sizeof(mag));
memset(&gyro, 0, sizeof(gyro));
rawcount = 0;
}
x = (float)data[0] * G_PER_COUNT;
y = (float)data[1] * G_PER_COUNT;
z = (float)data[2] * G_PER_COUNT;
accel.fGpFast[0] = x;
accel.fGpFast[1] = y;
accel.fGpFast[2] = y;
accel.fGp[0] += x;
accel.fGp[1] += y;
accel.fGp[2] += y;
x = (float)data[3] * DEG_PER_SEC_PER_COUNT;
y = (float)data[4] * DEG_PER_SEC_PER_COUNT;
z = (float)data[5] * DEG_PER_SEC_PER_COUNT;
gyro.fYp[0] += x;
gyro.fYp[1] += y;
gyro.fYp[2] += z;
gyro.iYpFast[rawcount][0] = data[3];
gyro.iYpFast[rawcount][1] = data[4];
gyro.iYpFast[rawcount][2] = data[5];
apply_calibration(data[6], data[7], data[8], &point);
mag.fBcFast[0] = point.x;
mag.fBcFast[1] = point.y;
mag.fBcFast[2] = point.z;
mag.fBc[0] += point.x;
mag.fBc[1] += point.y;
mag.fBc[2] += point.z;
rawcount++;
if (rawcount >= OVERSAMPLE_RATIO) {
ratio = 1.0f / (float)OVERSAMPLE_RATIO;
accel.fGp[0] *= ratio;
accel.fGp[1] *= ratio;
accel.fGp[2] *= ratio;
gyro.fYp[0] *= ratio;
gyro.fYp[1] *= ratio;
gyro.fYp[2] *= ratio;
mag.fBc[0] *= ratio;
mag.fBc[1] *= ratio;
mag.fBc[2] *= ratio;
fRun_9DOF_GBY_KALMAN(&fusionstate, &accel, &mag, &gyro,
&magcal, OVERSAMPLE_RATIO);
memcpy(&current_orientation, &(fusionstate.fqPl), sizeof(Quaternion_t));
}
}

15
visualize.c
View file

@ -46,17 +46,16 @@ static int sphere_region(float longitude, float latitude)
}
static void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out)
void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out)
{
float x, y, z;
x = ((float)rawx * 0.1) - magcal.fV[0];
y = ((float)rawy * 0.1) - magcal.fV[1];
z = ((float)rawz * 0.1) - magcal.fV[2];
x = ((float)rawx * UT_PER_COUNT) - magcal.fV[0];
y = ((float)rawy * UT_PER_COUNT) - magcal.fV[1];
z = ((float)rawz * UT_PER_COUNT) - magcal.fV[2];
out->x = x * magcal.finvW[0][0] + y * magcal.finvW[0][1] + z * magcal.finvW[0][2];
out->y = x * magcal.finvW[1][0] + y * magcal.finvW[1][1] + z * magcal.finvW[1][2];
out->z = x * magcal.finvW[2][0] + y * magcal.finvW[2][1] + z * magcal.finvW[2][2];
out->valid = 1;
}
static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix)
@ -78,12 +77,6 @@ static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix)
static void rotate(const Point_t *in, Point_t *out, const float *rmatrix)
{
if (out == NULL) return;
if (in == NULL || in->valid == 0) {
out->valid = 0;
out->x = out->y = out->z = 0;
return;
}
out->x = in->x * rmatrix[0] + in->y * rmatrix[1] + in->z * rmatrix[2];
out->y = in->x * rmatrix[3] + in->y * rmatrix[4] + in->z * rmatrix[5];
out->z = in->x * rmatrix[6] + in->y * rmatrix[7] + in->z * rmatrix[8];