From 2621b087a2c1355950a1fc579ddcf0181c183c96 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 28 Jun 2013 21:27:05 +0200 Subject: [PATCH] fixed filter in first bench test tries, works surprisingly well... --- .../modules/StateEstimation/filteraltitude.c | 29 ++++++++++++++----- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index 26e17569d..dc1cbb314 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -39,7 +39,7 @@ #define STACK_REQUIRED 128 -#define BARO_SENSOR_KP 0.05f +#define BARO_SENSOR_KP 0.03f #define ACCEL_DRIFT_KP 0.001f #define DT_ALPHA 1e-3f @@ -49,6 +49,7 @@ struct data { float pos[3]; // position updates from other filters float vel[3]; // position updates from other filters float dTA; + float dTA2; int32_t lastTime; float baroLast; int32_t baroLastTime; @@ -86,6 +87,7 @@ static int32_t init(stateFilter *self) this->vel[1] = 0.0f; this->vel[2] = 0.0f; this->dTA = -1; + this->dTA2 = -1; this->first_run = 1; return 0; } @@ -133,6 +135,10 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // correct velocity and position state (integration) // low pass for average dT, compensate timing jitter from scheduler float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f; + this->lastTime = PIOS_DELAY_GetRaw(); + if (dT < 0.001f) { + dT = 0.001f; + } if (this->dTA < 0) { this->dTA = dT; } else { @@ -142,13 +148,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->state[0] += this->state[1] * this->dTA; state->pos[0] = this->pos[0]; - state->pos[1] = this->pos[2]; + state->pos[1] = this->pos[1]; state->pos[2] = -this->state[0]; state->updated |= SENSORUPDATES_pos; state->vel[0] = this->vel[0]; - state->vel[1] = this->vel[2]; - state->vel[2] = -this->state[0]; + state->vel[1] = this->vel[1]; + state->vel[2] = -this->state[1]; state->updated |= SENSORUPDATES_vel; } if (IS_SET(state->updated, SENSORUPDATES_baro)) { @@ -156,21 +162,28 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->state[0] = (1.0f - BARO_SENSOR_KP) * this->state[0] + BARO_SENSOR_KP * state->baro[0]; // correct the velocity state (low pass differentiation) + // low pass for average dT, compensate timing jitter from scheduler float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f; + this->baroLastTime = PIOS_DELAY_GetRaw(); if (dT < 0.001f) { dT = 0.001f; } - this->state[1] = (1.0f - BARO_SENSOR_KP) * this->state[1] + BARO_SENSOR_KP * (state->baro[0] - this->baroLast) / dT; + if (this->dTA2 < 0) { + this->dTA2 = dT; + } else { + this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA; + } + this->state[1] = (1.0f - (BARO_SENSOR_KP * BARO_SENSOR_KP)) * this->state[1] + (BARO_SENSOR_KP * BARO_SENSOR_KP) * (state->baro[0] - this->baroLast) / this->dTA2; this->baroLast = state->baro[0]; state->pos[0] = this->pos[0]; - state->pos[1] = this->pos[2]; + state->pos[1] = this->pos[1]; state->pos[2] = -this->state[0]; state->updated |= SENSORUPDATES_pos; state->vel[0] = this->vel[0]; - state->vel[1] = this->vel[2]; - state->vel[2] = -this->state[0]; + state->vel[1] = this->vel[1]; + state->vel[2] = -this->state[1]; state->updated |= SENSORUPDATES_vel; } }