From e4b3e0462bfd23512aaed921eb27905d50ccaadd Mon Sep 17 00:00:00 2001 From: Erik Stromberg Date: Fri, 18 Nov 2016 14:24:59 +0100 Subject: [PATCH 1/4] Add tmp variables for state in prediction update in order not to overwrite previous time step while updating --- src/modules/src/estimator_kalman.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index fca8e2511c..81b559455e 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -637,10 +637,15 @@ static void stateEstimatorPredict(float cmdThrust, Axis3f *acc, Axis3f *gyro, fl S[STATE_Y] += R[1][0] * dx + R[1][1] * dy + R[1][2] * dz; S[STATE_Z] += R[2][0] * dx + R[2][1] * dy + R[2][2] * dz - GRAVITY_MAGNITUDE * dt2 / 2.0f; + // keep previous time step's state for the update + float tmpSPX = S[STATE_PX]; + float tmpSPY = S[STATE_PY]; + float tmpSPZ = S[STATE_PZ]; + // body-velocity update: accelerometers + gyros cross velocity - gravity in body frame - S[STATE_PX] += dt * (gyro->z * S[STATE_PY] - gyro->y * S[STATE_PZ] - GRAVITY_MAGNITUDE * R[2][0]); - S[STATE_PY] += dt * (gyro->z * S[STATE_PX] + gyro->x * S[STATE_PZ] - GRAVITY_MAGNITUDE * R[2][1]); - S[STATE_PZ] += dt * (zacc + gyro->y * S[STATE_PX] - gyro->x * S[STATE_PY] - GRAVITY_MAGNITUDE * R[2][2]); + S[STATE_PX] += dt * (gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * R[2][0]); + S[STATE_PY] += dt * (gyro->z * tmpSPX + gyro->x * tmpSPZ - GRAVITY_MAGNITUDE * R[2][1]); + S[STATE_PZ] += dt * (zacc + gyro->y * tmpSPX - gyro->x * tmpSPY - GRAVITY_MAGNITUDE * R[2][2]); } else // Acceleration can be in any direction, as measured by the accelerometer. This occurs, eg. in freefall or while being carried. { From 987dfc880e3dca832b28962db623508c0f83a852 Mon Sep 17 00:00:00 2001 From: Erik Stromberg Date: Fri, 18 Nov 2016 15:02:05 +0100 Subject: [PATCH 2/4] Change variables pertaining to state update in predict to static --- src/modules/src/estimator_kalman.c | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index 81b559455e..5505f51826 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -619,18 +619,22 @@ static void stateEstimatorPredict(float cmdThrust, Axis3f *acc, Axis3f *gyro, fl } quadIsFlying = (xTaskGetTickCount()-lastFlightCmd) < IN_FLIGHT_TIME_THRESHOLD; + static float dx, dy, dz; + static float tmpSPX, tmpSPY, tmpSPZ; + static float zacc; + if (quadIsFlying) // only acceleration in z direction { // TODO: In the next lines, can either use cmdThrust/mass, or acc->z. Need to test which is more reliable. // cmdThrust's error comes from poorly calibrated mass, and inexact cmdThrust -> thrust map // acc->z's error comes from measurement noise and accelerometer scaling // float zacc = cmdThrust; - float zacc = acc->z; + zacc = acc->z; // position updates in the body frame (will be rotated to inertial frame) - float dx = S[STATE_PX] * dt; - float dy = S[STATE_PY] * dt; - float dz = S[STATE_PZ] * dt + zacc * dt2 / 2.0f; // thrust can only be produced in the body's Z direction + dx = S[STATE_PX] * dt; + dy = S[STATE_PY] * dt; + dz = S[STATE_PZ] * dt + zacc * dt2 / 2.0f; // thrust can only be produced in the body's Z direction // position update S[STATE_X] += R[0][0] * dx + R[0][1] * dy + R[0][2] * dz; @@ -638,9 +642,9 @@ static void stateEstimatorPredict(float cmdThrust, Axis3f *acc, Axis3f *gyro, fl S[STATE_Z] += R[2][0] * dx + R[2][1] * dy + R[2][2] * dz - GRAVITY_MAGNITUDE * dt2 / 2.0f; // keep previous time step's state for the update - float tmpSPX = S[STATE_PX]; - float tmpSPY = S[STATE_PY]; - float tmpSPZ = S[STATE_PZ]; + tmpSPX = S[STATE_PX]; + tmpSPY = S[STATE_PY]; + tmpSPZ = S[STATE_PZ]; // body-velocity update: accelerometers + gyros cross velocity - gravity in body frame S[STATE_PX] += dt * (gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * R[2][0]); @@ -650,9 +654,9 @@ static void stateEstimatorPredict(float cmdThrust, Axis3f *acc, Axis3f *gyro, fl else // Acceleration can be in any direction, as measured by the accelerometer. This occurs, eg. in freefall or while being carried. { // position updates in the body frame (will be rotated to inertial frame) - float dx = S[STATE_PX] * dt + acc->x * dt2 / 2.0f; - float dy = S[STATE_PY] * dt + acc->y * dt2 / 2.0f; - float dz = S[STATE_PZ] * dt + acc->z * dt2 / 2.0f; // thrust can only be produced in the body's Z direction + dx = S[STATE_PX] * dt + acc->x * dt2 / 2.0f; + dy = S[STATE_PY] * dt + acc->y * dt2 / 2.0f; + dz = S[STATE_PZ] * dt + acc->z * dt2 / 2.0f; // thrust can only be produced in the body's Z direction // position update S[STATE_X] += R[0][0] * dx + R[0][1] * dy + R[0][2] * dz; From 32a0ed50bb6c7002984feacd776ce7cc841becd2 Mon Sep 17 00:00:00 2001 From: Erik Stromberg Date: Mon, 21 Nov 2016 09:53:02 +0100 Subject: [PATCH 3/4] Apply same overwrite fix to else-clause as well --- src/modules/src/estimator_kalman.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index 5505f51826..2908f50304 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -663,10 +663,15 @@ static void stateEstimatorPredict(float cmdThrust, Axis3f *acc, Axis3f *gyro, fl S[STATE_Y] += R[1][0] * dx + R[1][1] * dy + R[1][2] * dz; S[STATE_Z] += R[2][0] * dx + R[2][1] * dy + R[2][2] * dz - GRAVITY_MAGNITUDE * dt2 / 2.0f; + // keep previous time step's state for the update + tmpSPX = S[STATE_PX]; + tmpSPY = S[STATE_PY]; + tmpSPZ = S[STATE_PZ]; + // body-velocity update: accelerometers + gyros cross velocity - gravity in body frame - S[STATE_PX] += dt * (acc->x + gyro->z * S[STATE_PY] - gyro->y * S[STATE_PZ] - GRAVITY_MAGNITUDE * R[2][0]); - S[STATE_PY] += dt * (acc->y + gyro->z * S[STATE_PX] + gyro->x * S[STATE_PZ] - GRAVITY_MAGNITUDE * R[2][1]); - S[STATE_PZ] += dt * (acc->z + gyro->y * S[STATE_PX] - gyro->x * S[STATE_PY] - GRAVITY_MAGNITUDE * R[2][2]); + S[STATE_PX] += dt * (acc->x + gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * R[2][0]); + S[STATE_PY] += dt * (acc->y + gyro->z * tmpSPX + gyro->x * tmpSPZ - GRAVITY_MAGNITUDE * R[2][1]); + S[STATE_PZ] += dt * (acc->z + gyro->y * tmpSPX - gyro->x * tmpSPY - GRAVITY_MAGNITUDE * R[2][2]); } if(S[STATE_Z] < 0) { From df4abcf963dd02dfa42d605da74b4db0a8e39ee9 Mon Sep 17 00:00:00 2001 From: Arnaud Taffanel Date: Tue, 29 Nov 2016 10:31:31 +0100 Subject: [PATCH 4/4] #162 Remove static from float local variables --- src/modules/src/estimator_kalman.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index 0e173da95d..d3a148f241 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -619,9 +619,9 @@ static void stateEstimatorPredict(float cmdThrust, Axis3f *acc, Axis3f *gyro, fl } quadIsFlying = (xTaskGetTickCount()-lastFlightCmd) < IN_FLIGHT_TIME_THRESHOLD; - static float dx, dy, dz; - static float tmpSPX, tmpSPY, tmpSPZ; - static float zacc; + float dx, dy, dz; + float tmpSPX, tmpSPY, tmpSPZ; + float zacc; if (quadIsFlying) // only acceleration in z direction {