mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
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).
This commit is contained in:
James Cotton
parent
47fac7e31c
commit
900780e10c
@ -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;
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user