Get EKF running

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

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[0] = 1.0f;
-	Be[1] = 0;
+	Be[1] = 0.0f;
-	Be[2] = 0;		// local magnetic unit vector
+	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
+			P[i][j] = 0.0f; // zero all terms
-			F[i][j] = 0;
+			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;
+			H[j][i] = 0.0f;
-			K[i][j] = 0;
+			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[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;	// initial velocity variance (m/s)^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-5;	// initial quaternion variance
+	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-9;	// initial gyro bias variance (rad/s)^2
+	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[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f;	// initial pos and vel (m)
-	X[6] = 1;
+	X[6] = 1.0f;
-	X[7] = X[8] = X[9] = 0;	// initial quaternion (level and North) (m/s)
+	X[7] = X[8] = X[9] = 0.0f;	    // initial quaternion (level and North) (m/s)
-	X[10] = X[11] = X[12] = 0;	// initial gyro bias (rad/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[0] = Q[1] = Q[2] = 50e-4f;	// gyro noise variance (rad/s)^2
-	Q[3] = Q[4] = Q[5] = 0.01;	// accelerometer noise variance (m/s^2)^2
+	Q[3] = Q[4] = Q[5] = 0.00001f;	// 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[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[0] = R[1] = 0.004f;	// High freq GPS horizontal position noise variance (m^2)
-	R[2] = 0.036;		// High freq GPS vertical position noise variance (m^2)
+	R[2] = 0.036f;          // 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[3] = R[4] = 0.004f;   // High freq GPS horizontal velocity noise variance (m/s)^2
-	R[5] = 100;		// High freq GPS vertical 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.005;	// magnetometer unit vector noise variance
+	R[6] = R[7] = R[8] = 0.005f;    // magnetometer unit vector noise variance
-	R[9] = .05;		// High freq altimeter noise variance (m^2)
+	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;

flight/Modules/Attitude/revolution/attitude.c

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

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 = "<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 */,

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