rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Get EKF running

Browse Source
This commit is contained in:
James Cotton 2011-12-14 01:54:06 -06:00
parent ed7cbd5a6c
commit fd8899018f
5 changed files with 286 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
flight/Libraries/inc/insgps.h Normal file
View File

@ -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_ */

64
flight/Libraries/insgps13state.c
View File

@ -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<NUMX;i++){
if (PDiag != 0){
for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0;
P[i][j]=P[j][i]=0.0f;
P[i][i]=PDiag[i];
}
}
@ -239,7 +239,7 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
// EKF prediction step
LinearizeFG(X, U, F, G);
RungeKutta(X, U, dT);
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;
@ -323,7 +323,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
// magnetometer data in any units (use unit vector) and in body frame
Bmag =
sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] / Bmag;
Z[7] = mag_data[1] / Bmag;
@ -336,7 +336,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
LinearizeH(X, Be, H);
MeasurementEq(X, Be, Y);
SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
X[6] /= qmag;
X[7] /= qmag;
X[8] /= qmag;

161
flight/Modules/Attitude/revolution/attitude.c
View File

@ -56,11 +56,13 @@
#include "gyrosbias.h"
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "baroaltitude.h"
#include "flightstatus.h"
#include "homelocation.h"
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 1540
#define STACK_SIZE_BYTES 5540
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
#define FAILSAFE_TIMEOUT_MS 10
@ -75,6 +77,7 @@ static xTaskHandle attitudeTaskHandle;
static xQueueHandle gyroQueue;
static xQueueHandle accelQueue;
static xQueueHandle magQueue;
static xQueueHandle baroQueue;
const uint32_t SENSOR_QUEUE_SIZE = 10;
// Private functions
@ -83,6 +86,7 @@ static void AttitudeTask(void *parameters);
static int32_t updateSensors();
static int32_t updateAttitudeComplimentary(bool first_run);
static int32_t updateAttitudeINSGPS(bool first_run);
static void settingsUpdatedCb(UAVObjEvent * objEv);
static float accelKi = 0;
@ -147,13 +151,19 @@ int32_t AttitudeStart(void)
{
// Create the queues for the sensors
gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
GyrosConnectQueue(gyroQueue);
AccelsConnectQueue(accelQueue);
MagnetometerConnectQueue(magQueue);
BaroAltitudeConnectQueue(baroQueue);
return 0;
}
@ -171,13 +181,19 @@ static void AttitudeTask(void *parameters)
settingsUpdatedCb(AttitudeSettingsHandle());
bool first_run = true;
// Wait for all the sensors be to read
vTaskDelay(100);
// Main task loop
while (1) {
// This function blocks on data queue
updateAttitudeComplimentary(first_run);
if(0)
updateAttitudeComplimentary(first_run);
else
updateAttitudeINSGPS(first_run);
if (first_run)
first_run = false;
@ -199,7 +215,7 @@ static int32_t updateAttitudeComplimentary(bool first_run)
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
return -1;
}
@ -320,7 +336,138 @@ static int32_t updateAttitudeComplimentary(bool first_run)
return 0;
}
static void settingsUpdatedCb(UAVObjEvent * objEv) {
#include "insgps.h"
int32_t ins_failed = 0;
extern struct NavStruct Nav;
static int32_t updateAttitudeINSGPS(bool first_run)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
MagnetometerData magData;
BaroAltitudeData baroData;
static uint32_t ins_last_time = 0;
static bool inited;
if (first_run)
inited = false;
// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
if ( (xQueueReceive(gyroQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) ||
(xQueueReceive(accelQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) )
{
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
return -1;
}
// Get most recent data
// TODO: Acquire all data in a queue
GyrosGet(&gyrosData);
AccelsGet(&accelsData);
MagnetometerGet(&magData);
BaroAltitudeGet(&baroData);
bool mag_updated;
bool baro_updated;
if (inited) {
mag_updated = 0;
baro_updated = 0;
}
mag_updated |= xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
if (!inited && (!mag_updated || !baro_updated)) {
// Don't initialize until all sensors are read
return -1;
} else if (!inited ) {
inited = true;
float Rbe[3][3], q[4], accels[3], rpy[3], mag;
float ge[3]={0.0f,0.0f,-9.81f};
float zeros[3]={0.0f,0.0f,0.0f};
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
bool using_mags, using_gps;
INSGPSInit();
HomeLocationData home;
HomeLocationGet(&home);
RotFrom2Vectors(&accelsData.x, ge, &magData.x, home.Be, Rbe);
R2Quaternion(Rbe,q);
INSSetState(zeros, zeros, q, zeros, zeros);
INSResetP(Pdiag);
/* GyrosBiasData gyrosBias;
gyrosBias.x = gyrosData.x;
gyrosBias.y = gyrosData.y;
gyrosBias.z = gyrosData.z;
GyrosBiasSet(&gyrosBias);*/
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
// Perform the update
static uint32_t updated_without_gps = 0;
float zeros[3] = {0, 0, 0};
uint16_t sensors = 0;
float dT;
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 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);

6
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -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 = "<group>"; };
65F53D2E14984BB300A62D5D /* insgps_helper.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = insgps_helper.c; sourceTree = "<group>"; };
65F53D2F14984BB300A62D5D /* insgps13state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = insgps13state.c; sourceTree = "<group>"; };
65F53D3014984BB300A62D5D /* insgps16state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = insgps16state.c; sourceTree = "<group>"; };
65F93C3912EE09280047DB36 /* aes.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = aes.c; sourceTree = "<group>"; };
65F93C3B12EE09280047DB36 /* aes.lst */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = aes.lst; sourceTree = "<group>"; };
65F93C3C12EE09280047DB36 /* aes.o */ = {isa = PBXFileReference; lastKnownFileType = "compiled.mach-o.objfile"; path = aes.o; sourceTree = "<group>"; };
@ -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 */,

1
flight/Revolution/Makefile
View File

@ -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