From 900780e10c7a8feb48fb40811fc0cc7f94a9c447 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 19 Feb 2012 11:34:12 -0600 Subject: [PATCH] Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased). --- flight/Modules/AltitudeHold/altitudehold.c | 120 ++++++++++++++------- matlab/revo/async_attitude.m | 22 ++-- matlab/revo/generate_altitude.m | 58 +++++----- 3 files changed, 124 insertions(+), 76 deletions(-) diff --git a/flight/Modules/AltitudeHold/altitudehold.c b/flight/Modules/AltitudeHold/altitudehold.c index 39309fd7e..81cc9f4f4 100644 --- a/flight/Modules/AltitudeHold/altitudehold.c +++ b/flight/Modules/AltitudeHold/altitudehold.c @@ -120,7 +120,6 @@ float switchThrottle; float smoothed_altitude; float starting_altitude; -int32_t loop_count; /** * Module thread, should not return. */ @@ -142,9 +141,7 @@ static void altitudeHoldTask(void *parameters) // Main task loop lastSysTime = xTaskGetTickCount(); - while (1) { - loop_count++; - + while (1) { bool baro_updated; // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE ) @@ -177,14 +174,15 @@ static void altitudeHoldTask(void *parameters) running = false; } else if (ev.obj == AccelsHandle()) { static uint32_t timeval; + + static float z[4] = {0, 0, 0, 0}; + float z_new[4]; + float P[4][4], K[4][2], x[2]; + float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7}; + static float V[4][4] = {{10.0f, 0, 0, 0}, {0, 100.0f, 0, 0}, {0, 0, 100.0f, 0}, {0, 0, 0, 1000.0f}}; float dT; - static float z[3] = {0, 0, 0}; - float z_new[3]; - float P[3][3], K[3][2], x[2]; - static float G[3] = {1.0e-5f, 1.0e-5f, 1.0e-3f}; static float S[2] = {1.0f,10.0f}; - static float V[3] = {10.0f, 10.0f, 10.0f}; thisTime = xTaskGetTickCount(); @@ -203,7 +201,8 @@ static void altitudeHoldTask(void *parameters) if (init == WAITIING_INIT) { z[0] = baro.Altitude; z[1] = 0; - z[2] = 0; + z[2] = accels.z; + z[3] = 0; init = INITED; } else if (init == WAITING_BARO) continue; @@ -225,46 +224,89 @@ static void altitudeHoldTask(void *parameters) dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f; timeval = PIOS_DELAY_GetRaw(); - V[0] = 0.014f; - V[1] = 9.6f; - V[2] = 3.2e-3f; - - P[0][0] = G[0]+V[0]+(dT*dT)*V[1]; - P[1][1] = G[1]+V[1]+(dT*dT)*V[2]; - P[2][2] = G[2]+V[2]; - P[1][0] = P[0][1] = dT*V[1]; - P[2][1] = P[1][2] = dT*V[2]; + P[0][0] = dT*(V[0][1]+dT*V[1][1])+V[0][0]+G[0]+dT*V[1][0]; + P[0][1] = dT*(V[0][2]+dT*V[1][2])+V[0][1]+dT*V[1][1]; + P[0][2] = V[0][2]+dT*V[1][2]; + P[0][3] = V[0][3]+dT*V[1][3]; + P[1][0] = dT*(V[1][1]+dT*V[2][1])+V[1][0]+dT*V[2][0]; + P[1][1] = dT*(V[1][2]+dT*V[2][2])+V[1][1]+G[1]+dT*V[2][1]; + P[1][2] = V[1][2]+dT*V[2][2]; + P[1][3] = V[1][3]+dT*V[2][3]; + P[2][0] = V[2][0]+dT*V[2][1]; + P[2][1] = V[2][1]+dT*V[2][2]; + P[2][2] = V[2][2]+G[2]; + P[2][3] = V[2][3]; + P[3][0] = V[3][0]+dT*V[3][1]; + P[3][1] = V[3][1]+dT*V[3][2]; + P[3][2] = V[3][2]; + P[3][3] = V[3][3]+G[3]; if (baro_updated) { - K[0][0] = -S[0]/(G[0]+S[0]+V[0]+(dT*dT)*V[1])+1.0f; - K[1][0] = (dT*V[1])/(G[0]+S[0]+V[0]+(dT*dT)*V[1]); - K[1][1] = (dT*V[2])/(G[2]+S[1]+V[2]); - K[2][1] = -S[1]/(G[2]+S[1]+V[2])+1.0f; - - z_new[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]+z[0]; - z_new[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]+K[1][1]*(x[1]-z[2])+z[1]; - z_new[2] = K[2][1]*(x[1]-z[2])+z[2]; + K[0][0] = -(V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])+1.0f; + K[0][1] = ((V[0][2]+V[0][3])*S[0]+dT*(V[1][2]+V[1][3])*S[0])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); + K[1][0] = (V[1][0]*G[2]+V[1][0]*G[3]+V[1][0]*S[1]+V[1][0]*V[2][2]-V[2][0]*V[1][2]+V[1][0]*V[2][3]+V[1][0]*V[3][2]-V[2][0]*V[1][3]-V[1][2]*V[3][0]+V[1][0]*V[3][3]-V[3][0]*V[1][3]+(dT*dT)*V[2][1]*V[3][2]-(dT*dT)*V[2][2]*V[3][1]+(dT*dT)*V[2][1]*V[3][3]-(dT*dT)*V[3][1]*V[2][3]+dT*V[1][1]*G[2]+dT*V[2][0]*G[2]+dT*V[1][1]*G[3]+dT*V[2][0]*G[3]+dT*V[1][1]*S[1]+dT*V[2][0]*S[1]+(dT*dT)*V[2][1]*G[2]+(dT*dT)*V[2][1]*G[3]+(dT*dT)*V[2][1]*S[1]+dT*V[1][1]*V[2][2]-dT*V[1][2]*V[2][1]+dT*V[1][1]*V[2][3]+dT*V[1][1]*V[3][2]+dT*V[2][0]*V[3][2]-dT*V[1][2]*V[3][1]-dT*V[2][1]*V[1][3]-dT*V[3][0]*V[2][2]+dT*V[1][1]*V[3][3]+dT*V[2][0]*V[3][3]-dT*V[3][0]*V[2][3]-dT*V[1][3]*V[3][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); + K[1][1] = (V[1][2]*G[0]+V[1][3]*G[0]+V[1][2]*S[0]+V[1][3]*S[0]+V[0][0]*V[1][2]-V[1][0]*V[0][2]+V[0][0]*V[1][3]-V[1][0]*V[0][3]+(dT*dT)*V[0][1]*V[2][2]+(dT*dT)*V[1][0]*V[2][2]-(dT*dT)*V[0][2]*V[2][1]-(dT*dT)*V[2][0]*V[1][2]+(dT*dT)*V[0][1]*V[2][3]+(dT*dT)*V[1][0]*V[2][3]-(dT*dT)*V[2][0]*V[1][3]-(dT*dT)*V[0][3]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][2]-(dT*dT*dT)*V[1][2]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][3]-(dT*dT*dT)*V[2][1]*V[1][3]+dT*V[2][2]*G[0]+dT*V[2][3]*G[0]+dT*V[2][2]*S[0]+dT*V[2][3]*S[0]+dT*V[0][0]*V[2][2]+dT*V[0][1]*V[1][2]-dT*V[0][2]*V[1][1]-dT*V[0][2]*V[2][0]+dT*V[0][0]*V[2][3]+dT*V[0][1]*V[1][3]-dT*V[1][1]*V[0][3]-dT*V[2][0]*V[0][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); + K[2][0] = (V[2][0]*G[3]-V[3][0]*G[2]+V[2][0]*S[1]+V[2][0]*V[3][2]-V[3][0]*V[2][2]+V[2][0]*V[3][3]-V[3][0]*V[2][3]+dT*V[2][1]*G[3]-dT*V[3][1]*G[2]+dT*V[2][1]*S[1]+dT*V[2][1]*V[3][2]-dT*V[2][2]*V[3][1]+dT*V[2][1]*V[3][3]-dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); + K[2][1] = (V[0][0]*G[2]+V[2][2]*G[0]+V[2][3]*G[0]+V[2][2]*S[0]+V[2][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]-V[2][0]*V[0][3]+G[0]*G[2]+G[2]*S[0]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]-(dT*dT)*V[2][1]*V[1][3]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+(dT*dT)*V[1][1]*G[2]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[1][0]*V[2][3]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); + K[3][0] = (-V[2][0]*G[3]+V[3][0]*G[2]+V[3][0]*S[1]-V[2][0]*V[3][2]+V[3][0]*V[2][2]-V[2][0]*V[3][3]+V[3][0]*V[2][3]-dT*V[2][1]*G[3]+dT*V[3][1]*G[2]+dT*V[3][1]*S[1]-dT*V[2][1]*V[3][2]+dT*V[2][2]*V[3][1]-dT*V[2][1]*V[3][3]+dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); + K[3][1] = (V[0][0]*G[3]+V[3][2]*G[0]+V[3][3]*G[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[3][2]-V[0][2]*V[3][0]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[3]+G[3]*S[0]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+(dT*dT)*V[1][1]*G[3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]); + + z_new[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]-K[0][1]*(-x[1]+z[2]+z[3])+z[0]; + z_new[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]-K[1][1]*(-x[1]+z[2]+z[3])+z[1]; + z_new[2] = -K[2][0]*(dT*z[1]-x[0]+z[0])-K[2][1]*(-x[1]+z[2]+z[3])+z[2]; + z_new[3] = -K[3][0]*(dT*z[1]-x[0]+z[0])-K[3][1]*(-x[1]+z[2]+z[3])+z[3]; memcpy(z, z_new, sizeof(z_new)); - - V[0] = -P[0][0]*(K[0][0]-1.0f); - V[1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1]; - V[2] = -P[2][2]*(K[2][1]-1.0f); + + V[0][0] = -K[0][1]*P[2][0]-K[0][1]*P[3][0]-P[0][0]*(K[0][0]-1.0f); + V[0][1] = -K[0][1]*P[2][1]-K[0][1]*P[3][2]-P[0][1]*(K[0][0]-1.0f); + V[0][2] = -K[0][1]*P[2][2]-K[0][1]*P[3][2]-P[0][2]*(K[0][0]-1.0f); + V[0][3] = -K[0][1]*P[2][3]-K[0][1]*P[3][3]-P[0][3]*(K[0][0]-1.0f); + V[1][0] = P[1][0]-K[1][0]*P[0][0]-K[1][1]*P[2][0]-K[1][1]*P[3][0]; + V[1][1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1]-K[1][1]*P[3][2]; + V[1][2] = P[1][2]-K[1][0]*P[0][2]-K[1][1]*P[2][2]-K[1][1]*P[3][2]; + V[1][3] = P[1][3]-K[1][0]*P[0][3]-K[1][1]*P[2][3]-K[1][1]*P[3][3]; + V[2][0] = -K[2][0]*P[0][0]-K[2][1]*P[3][0]-P[2][0]*(K[2][1]-1.0f); + V[2][1] = -K[2][0]*P[0][1]-K[2][1]*P[3][2]-P[2][1]*(K[2][1]-1.0f); + V[2][2] = -K[2][0]*P[0][2]-K[2][1]*P[3][2]-P[2][2]*(K[2][1]-1.0f); + V[2][3] = -K[2][0]*P[0][3]-K[2][1]*P[3][3]-P[2][3]*(K[2][1]-1.0f); + V[3][0] = -K[3][0]*P[0][0]-K[3][1]*P[2][0]-P[3][0]*(K[3][1]-1.0f); + V[3][1] = -K[3][0]*P[0][1]-K[3][1]*P[2][1]-P[3][2]*(K[3][1]-1.0f); + V[3][2] = -K[3][0]*P[0][2]-K[3][1]*P[2][2]-P[3][2]*(K[3][1]-1.0f); + V[3][3] = -K[3][0]*P[0][3]-K[3][1]*P[2][3]-P[3][3]*(K[3][1]-1.0f); + baro_updated = false; } else { - K[1][0] = (dT*V[2])/(G[2]+S[1]+V[2]); - K[2][0] = -S[1]/(G[2]+S[1]+V[2])+1.0f; + K[0][0] = (V[0][2]+V[0][3]+dT*V[1][2]+dT*V[1][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); + K[1][0] = (V[1][2]+V[1][3]+dT*V[2][2]+dT*V[2][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); + K[2][0] = (V[2][2]+V[2][3]+G[2])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); + K[3][0] = (V[3][2]+V[3][3]+G[3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]); + - z_new[0] = dT*z[1]+z[0]; - z_new[1] = dT*z[2]+K[1][0]*(x[1]-z[2])+z[1]; - z_new[2] = K[2][0]*(x[1]-z[2])+z[2]; + z_new[0] = dT*z[1]-K[0][0]*(-x[1]+z[2]+z[3])+z[0]; + z_new[1] = dT*z[2]-K[1][0]*(-x[1]+z[2]+z[3])+z[1]; + z_new[2] = -K[2][0]*(-x[1]+z[2]+z[3])+z[2]; + z_new[3] = -K[3][0]*(-x[1]+z[2]+z[3])+z[3]; memcpy(z, z_new, sizeof(z_new)); - V[0] = P[0][0]; - V[1] = P[1][1]-K[1][0]*P[2][1]; - V[2] = -P[2][2]*(K[2][0]-1.0f); + V[0][0] = P[0][0]-K[0][0]*P[2][0]-K[0][0]*P[3][0]; + V[0][1] = P[0][1]-K[0][0]*P[2][1]-K[0][0]*P[3][2]; + V[0][2] = P[0][2]-K[0][0]*P[2][2]-K[0][0]*P[3][2]; + V[0][3] = P[0][3]-K[0][0]*P[2][3]-K[0][0]*P[3][3]; + V[1][0] = P[1][0]-K[1][0]*P[2][0]-K[1][0]*P[3][0]; + V[1][1] = P[1][1]-K[1][0]*P[2][1]-K[1][0]*P[3][2]; + V[1][2] = P[1][2]-K[1][0]*P[2][2]-K[1][0]*P[3][2]; + V[1][3] = P[1][3]-K[1][0]*P[2][3]-K[1][0]*P[3][3]; + V[2][0] = -K[2][0]*P[3][0]-P[2][0]*(K[2][0]-1.0f); + V[2][1] = -K[2][0]*P[3][2]-P[2][1]*(K[2][0]-1.0f); + V[2][2] = -K[2][0]*P[3][2]-P[2][2]*(K[2][0]-1.0f); + V[2][3] = -K[2][0]*P[3][3]-P[2][3]*(K[2][0]-1.0f); + V[3][0] = -K[3][0]*P[2][0]-P[3][0]*(K[3][0]-1.0f); + V[3][1] = -K[3][0]*P[2][1]-P[3][2]*(K[3][0]-1.0f); + V[3][2] = -K[3][0]*P[2][2]-P[3][2]*(K[3][0]-1.0f); + V[3][3] = -K[3][0]*P[2][3]-P[3][3]*(K[3][0]-1.0f); } AltHoldSmoothedData altHold; diff --git a/matlab/revo/async_attitude.m b/matlab/revo/async_attitude.m index 658575b5e..ebbe4801f 100644 --- a/matlab/revo/async_attitude.m +++ b/matlab/revo/async_attitude.m @@ -2,11 +2,13 @@ baro = fixTime(BaroAltitude); accel = fixTime(Accels); attitude = fixTime(AttitudeActual); +%accel.z(end/2:end) = accel.z(end/2:end) + 1; +accel.z = accel.z+2; -Gamma = diag([1e-5 1e-5 0.00001 1e-20]); % state noise -accel_sigma = 1; -baro_sigma = 1; -Nu = diag([10 10 10 10]); +Gamma = diag([1e-15 1e-15 1e-3 1e-5]); % state noise +accel_sigma = 10; +baro_sigma = 0.1; +Nu = diag([10 10 10 1000]); z = zeros(length(accel.z),4); Nu_n = zeros([4 4 length(accel.z)]); Nu_n(:,:,1) = Nu; @@ -19,7 +21,7 @@ i = 1; timestamp = []; z(1) = baro.Altitude(1); -z(1:5,4) = 0.4; +z(1:5,4) = 0; timestamp(1) = t; log_accel = 0; while (last_accel_idx + 1) <= length(accel.z) && (last_baro_idx + 1) <= length(baro.Altitude) @@ -44,12 +46,12 @@ while (last_accel_idx + 1) <= length(accel.z) && (last_baro_idx + 1) <= length(b x = [baro.Altitude(last_baro_idx + 1); -accel_ned-9.81]; last_baro_idx = last_baro_idx + 1; last_accel_idx = last_accel_idx + 1; - C = [1 0 0 0; 0 0 1 -1]; + C = [1 0 0 0; 0 0 1 1]; Sigma = diag([baro_sigma; accel_sigma]); elseif update_accel x = -accel_ned - 9.81; last_accel_idx = last_accel_idx + 1; - C = [0 0 1 -1]; + C = [0 0 1 1]; Sigma = [accel_sigma]; elseif update_baro x = [baro.Altitude(last_baro_idx + 1)]; @@ -71,10 +73,7 @@ while (last_accel_idx + 1) <= length(accel.z) && (last_baro_idx + 1) <= length(b last_t = t; i = i + 1; - - % Zero out non-diag covariance - Nu = diag(diag(Nu)); - + A = [1 dT 0 0; 0 1 dT 0; 0 0 1 0; 0 0 0 1]; P = A * Nu * A' + Gamma; @@ -85,7 +84,6 @@ while (last_accel_idx + 1) <= length(accel.z) && (last_baro_idx + 1) <= length(b Nu = (eye(4) - K * C) * P; Nu_n(:,:,i) = Nu; - z(i,4) = 0; if mod(i,10000) == 0 subplot(311) plot(baro.timestamp, baro.Altitude, '.', timestamp(1:i),z(1:i,1),'r','LineWidth',5) diff --git a/matlab/revo/generate_altitude.m b/matlab/revo/generate_altitude.m index 09ac5ae1b..ef0f186aa 100644 --- a/matlab/revo/generate_altitude.m +++ b/matlab/revo/generate_altitude.m @@ -1,27 +1,39 @@ % Generate the symbolic code for the kalman filter on altitude dT = sym('dT','real'); -A = [1 dT 0; 0 1 dT; 0 0 1]; -Nu = diag([sym('V[1]') sym('V[2]') sym('V[3]')]); +A = [1 dT 0 0; 0 1 dT 0; 0 0 1 0; 0 0 0 1]; +%Nu = diag([sym('V[1]') sym('V[2]') sym('V[3]') sym('V[4]')]); +%Nu = [sym('V[1][1]') 0 0 0; ... +% 0 sym('V[2][2]') 0 0; ... +% 0 0 sym('V[3][3]') sym('V[3][4]'); ... +% 0 0 sym('V[4][3]') sym('V[4][4]')]; +Nu = [sym('V[1][1]') sym('V[1][2]') sym('V[1][3]') sym('V[1][4]'); ... + sym('V[2][1]') sym('V[2][2]') sym('V[2][3]') sym('V[2][4]'); ... + sym('V[3][1]') sym('V[3][2]') sym('V[3][3]') sym('V[3][4]'); ... + sym('V[4][1]') sym('V[4][2]') sym('V[4][3]') sym('V[4][4]');]; -Gamma = diag([sym('G[1]') sym('G[2]') sym('G[3]')]); + +Gamma = diag([sym('G[1]') sym('G[2]') sym('G[3]') sym('G[4]')]); Sigma = diag([sym('S[1]') sym('S[2]')]); -C = [1 0 0; 0 0 1]; -state = [sym('z[1]'); sym('z[2]'); sym('z[3]')]; +C = [1 0 0 0; 0 0 1 1]; +state = [sym('z[1]'); sym('z[2]'); sym('z[3]'); sym('z[4]')]; measure = [sym('x[1]'); sym('x[2]')]; P = simplify(A * Nu * A' + Gamma); K = simplify(P*C'*(C*P*C'+Sigma)^-1); -P_mat = [sym('P[1][1]') sym('P[1][2]') sym('P[1][3]'); ... - sym('P[2][1]') sym('P[2][2]') sym('P[2][3]'); ... - sym('P[3][1]') sym('P[3][2]') sym('P[3][3]')]; +% fill in the zeros from above equations to make next calculation sparse +P_mat = [sym('P[1][1]') sym('P[1][2]') sym('P[1][3]') sym('P[1][4]'); ... + sym('P[2][1]') sym('P[2][2]') sym('P[2][3]') sym('P[2][4]'); ... + sym('P[3][1]') sym('P[3][2]') sym('P[3][3]') sym('P[3][4]'); ... + sym('P[4][1]') sym('P[4][3]') sym('P[4][3]') sym('P[4][4]')]; K_mat = [sym('K[1][1]') sym('K[1][2]'); ... - sym('K[2][1]') sym('K[2][2]'); ... - sym('K[3][1]') sym('K[3][2]')]; + sym('K[2][1]') sym('K[2][2]'); ... + sym('K[3][1]') sym('K[3][2]'); ... + sym('K[4][1]') sym('K[4][2]')]; z_new = A * state + K_mat * (measure - C * A * state); -V = (eye(3) - K_mat * C) * P_mat; +V = (eye(4) - K_mat * C) * P_mat; ccode(P) ccode(K) @@ -31,29 +43,25 @@ ccode(V) %% For when there is no baro update % Generate the symbolic code for the kalman filter on altitude - -dT = sym('dT','real'); -A = [1 dT 0; 0 1 dT; 0 0 1]; -Nu = diag([sym('V[1]') sym('V[2]') sym('V[3]')]); - -Gamma = diag([sym('G[1]') sym('G[2]') sym('G[3]')]); +C = [0 0 1 1]; Sigma = sym('S[2]'); -C = [0 0 1]; -state = [sym('z[1]'); sym('z[2]'); sym('z[3]')]; measure = [sym('x[2]')]; P = simplify(A * Nu * A' + Gamma); K = simplify(P*C'*(C*P*C'+Sigma)^-1); -P_mat = [sym('P[1][1]') sym('P[1][2]') sym('P[1][3]'); ... - sym('P[2][1]') sym('P[2][2]') sym('P[2][3]'); ... - sym('P[3][1]') sym('P[3][2]') sym('P[3][3]')]; +% fill in the zeros from above equations to make next calculation sparse +P_mat = [sym('P[1][1]') sym('P[1][2]') sym('P[1][3]') sym('P[1][4]'); ... + sym('P[2][1]') sym('P[2][2]') sym('P[2][3]') sym('P[2][4]'); ... + sym('P[3][1]') sym('P[3][2]') sym('P[3][3]') sym('P[3][4]'); ... + sym('P[4][1]') sym('P[4][3]') sym('P[4][3]') sym('P[4][4]')]; K_mat = [sym('K[1][1]'); ... - sym('K[2][1]'); ... - sym('K[3][1]')]; + sym('K[2][1]'); ... + sym('K[3][1]'); ... + sym('K[4][1]')]; z_new = A * state + K_mat * (measure - C * A * state); -V = (eye(3) - K_mat * C) * P_mat; +V = (eye(4) - K_mat * C) * P_mat; ccode(P) ccode(K)