From fd8899018fd86adc63e29a6ecc7e13b7edb96205 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 14 Dec 2011 01:54:06 -0600 Subject: [PATCH] Get EKF running --- flight/Libraries/inc/insgps.h | 93 ++++++++++ flight/Libraries/insgps13state.c | 64 +++---- flight/Modules/Attitude/revolution/attitude.c | 161 +++++++++++++++++- .../OpenPilotOSX.xcodeproj/project.pbxproj | 6 + flight/Revolution/Makefile | 1 + 5 files changed, 286 insertions(+), 39 deletions(-) create mode 100644 flight/Libraries/inc/insgps.h diff --git a/flight/Libraries/inc/insgps.h b/flight/Libraries/inc/insgps.h new file mode 100644 index 000000000..bf4faae4b --- /dev/null +++ b/flight/Libraries/inc/insgps.h @@ -0,0 +1,93 @@ +/** + ****************************************************************************** + * @addtogroup AHRS + * @{ + * @addtogroup INSGPS + * @{ + * @brief INSGPS is a joint attitude and position estimation EKF + * + * @file insgps.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Include file of the INSGPS exposed functionality. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef INSGPS_H_ +#define INSGPS_H_ + +#include "stdint.h" + +/** + * @addtogroup Constants + * @{ + */ +#define POS_SENSORS 0x007 +#define HORIZ_SENSORS 0x018 +#define VERT_SENSORS 0x020 +#define MAG_SENSORS 0x1C0 +#define BARO_SENSOR 0x200 + +#define FULL_SENSORS 0x3FF + +/** + * @} + */ + +// Exposed Function Prototypes +void INSGPSInit(); +void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT); +void INSCovariancePrediction(float dT); +void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed); + +void INSResetP(float PDiag[13]); +void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]); +void INSSetPosVelVar(float PosVar, float VelVar); +void INSSetGyroBias(float gyro_bias[3]); +void INSSetAccelVar(float accel_var[3]); +void INSSetGyroVar(float gyro_var[3]); +void INSSetMagNorth(float B[3]); +void INSSetMagVar(float scaled_mag_var[3]); +void INSPosVelReset(float pos[3], float vel[3]); + +void MagCorrection(float mag_data[3]); +void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt); +void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], + float BaroAlt); +void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt); +void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]); +void VelBaroCorrection(float Vel[3], float BaroAlt); + +uint16_t ins_get_num_states(); + +// Nav structure containing current solution +struct NavStruct { + float Pos[3]; // Position in meters and relative to a local NED frame + float Vel[3]; // Velocity in meters and in NED + float q[4]; // unit quaternion rotation relative to NED + float gyro_bias[3]; + float accel_bias[3]; +} Nav; + +/** + * @} + * @} + */ + +#endif /* EKF_H_ */ diff --git a/flight/Libraries/insgps13state.c b/flight/Libraries/insgps13state.c index e2868bbd6..a9e45df56 100644 --- a/flight/Libraries/insgps13state.c +++ b/flight/Libraries/insgps13state.c @@ -76,52 +76,52 @@ uint16_t ins_get_num_states() void INSGPSInit() //pretty much just a place holder for now { - Be[0] = 1; - Be[1] = 0; - Be[2] = 0; // local magnetic unit vector + Be[0] = 1.0f; + Be[1] = 0.0f; + Be[2] = 0.0f; // local magnetic unit vector for (int i = 0; i < NUMX; i++) { for (int j = 0; j < NUMX; j++) { - P[i][j] = 0; // zero all terms - F[i][j] = 0; + P[i][j] = 0.0f; // zero all terms + F[i][j] = 0.0f; } for (int j = 0; j < NUMW; j++) - G[i][j] = 0; + G[i][j] = 0.0f; for (int j = 0; j < NUMV; j++) { - H[j][i] = 0; - K[i][j] = 0; + H[j][i] = 0.0f; + K[i][j] = 0.0f; } - X[i] = 0; + X[i] = 0.0f; } for (int i = 0; i < NUMW; i++) - Q[i] = 0; + Q[i] = 0.0f; for (int i = 0; i < NUMV; i++) - R[i] = 0; + R[i] = 0.0f; - P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2) - P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2 - P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance - P[10][10] = P[11][11] = P[12][12] = 1e-9; // initial gyro bias variance (rad/s)^2 + P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2) + P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance + P[10][10] = P[11][11] = P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2 - X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m) - X[6] = 1; - X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s) - X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s) + X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m) + X[6] = 1.0f; + X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s) + X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s) - Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2 - Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2 - Q[6] = Q[7] = Q[8] = 2e-15; // gyro bias random walk variance (rad/s^2)^2 + Q[0] = Q[1] = Q[2] = 50e-4f; // gyro noise variance (rad/s)^2 + Q[3] = Q[4] = Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2 + Q[6] = Q[7] = Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2 - R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2) - R[2] = 0.036; // High freq GPS vertical position noise variance (m^2) - R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2 - R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2 - R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance - R[9] = .05; // High freq altimeter noise variance (m^2) + R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) + R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) + R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 + R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 + R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance + R[9] = .05f; // High freq altimeter noise variance (m^2) } void INSResetP(float PDiag[NUMX]) @@ -132,7 +132,7 @@ void INSResetP(float PDiag[NUMX]) for (i=0;i 0.01f) + dT = 0.01f; + else if(dT <= 0.001f) + dT = 0.001f; + + float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f}; + + // Advance the state estimate + INSStatePrediction(gyros, &accelsData.x, dT); + + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + attitude.q1 = Nav.q[0]; + attitude.q2 = Nav.q[1]; + attitude.q3 = Nav.q[2]; + attitude.q4 = Nav.q[3]; + Quaternion2RPY(&attitude.q1,&attitude.Roll); + AttitudeActualSet(&attitude); + + // Advance the covariance estimate + INSCovariancePrediction(dT); + + /* Indoors, update with zero position and velocity and high covariance */ + sensors = HORIZ_SENSORS | VERT_SENSORS; + + if(mag_updated) + sensors |= MAG_SENSORS; + if(baro_updated) + sensors |= BARO_SENSOR; + + /* + * TODO: Need to add a general sanity check for all the inputs to make sure their kosher + * although probably should occur within INS itself + */ + INSCorrection(&magData.x, zeros, zeros, baroData.Altitude, sensors); + + if(fabs(Nav.gyro_bias[0]) > 0.1f || fabs(Nav.gyro_bias[1]) > 0.1f || fabs(Nav.gyro_bias[2]) > 0.1f) { + float zeros[3] = {0.0f,0.0f,0.0f}; + INSSetGyroBias(zeros); + } + +} + +static void settingsUpdatedCb(UAVObjEvent * objEv) +{ AttitudeSettingsData attitudeSettings; AttitudeSettingsGet(&attitudeSettings); diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index ee6cdd6c0..d435e3d14 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -3104,6 +3104,9 @@ 65E8F0EE11EFF25C00BBF654 /* startup_stm32f10x_HD.S */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.asm; name = startup_stm32f10x_HD.S; path = ../../PiOS/STM32F10x/startup_stm32f10x_HD.S; sourceTree = SOURCE_ROOT; }; 65E8F0F111EFF25C00BBF654 /* startup_stm32f10x_MD.S */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.asm; name = startup_stm32f10x_MD.S; path = ../../PiOS/STM32F10x/startup_stm32f10x_MD.S; sourceTree = SOURCE_ROOT; }; 65EA2E171273C55200636061 /* ratedesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ratedesired.xml; sourceTree = ""; }; + 65F53D2E14984BB300A62D5D /* insgps_helper.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = insgps_helper.c; sourceTree = ""; }; + 65F53D2F14984BB300A62D5D /* insgps13state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = insgps13state.c; sourceTree = ""; }; + 65F53D3014984BB300A62D5D /* insgps16state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = insgps16state.c; sourceTree = ""; }; 65F93C3912EE09280047DB36 /* aes.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = aes.c; sourceTree = ""; }; 65F93C3B12EE09280047DB36 /* aes.lst */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = aes.lst; sourceTree = ""; }; 65F93C3C12EE09280047DB36 /* aes.o */ = {isa = PBXFileReference; lastKnownFileType = "compiled.mach-o.objfile"; path = aes.o; sourceTree = ""; }; @@ -3820,6 +3823,9 @@ 657CEEB6121DBC63007A1FBE /* Libraries */ = { isa = PBXGroup; children = ( + 65F53D2E14984BB300A62D5D /* insgps_helper.c */, + 65F53D2F14984BB300A62D5D /* insgps13state.c */, + 65F53D3014984BB300A62D5D /* insgps16state.c */, 6549E0D21279B3C800C5476F /* fifo_buffer.c */, 651913371256C5240039C0A3 /* ahrs_comm_objects.c */, 651913381256C5240039C0A3 /* ahrs_spi_comm.c */, diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index bd7f20b70..bbd1ab392 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -130,6 +130,7 @@ SRC += $(OPSYSTEM)/cm3_fault_handlers.c SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c +SRC += $(FLIGHTLIB)/insgps13state.c ## PIOS Hardware (STM32F4xx) include $(PIOS)/STM32F4xx/library.mk