Skip to content

Commit

Permalink
#495 Refactoring
Browse files Browse the repository at this point in the history
  • Loading branch information
@krichardsson
krichardsson committed Nov 1, 2019
1 parent 866903e commit 30e9575
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 65 deletions.
8 changes: 4 additions & 4 deletions src/modules/interface/kalman_core.h
View file
Expand Up @@ -102,7 +102,7 @@ void kalmanCoreInit(kalmanCoreData_t* this);
/* - Measurement updates based on sensors */

// Barometer
void kalmanCoreUpdateWithBaro(kalmanCoreData_t* this, baro_t *baro, bool quadIsFlying);
void kalmanCoreUpdateWithBaro(kalmanCoreData_t* this, float baroAsl, bool quadIsFlying);

// Absolute height measurement along the room Z
void kalmanCoreUpdateWithAbsoluteHeight(kalmanCoreData_t* this, heightMeasurement_t* height);
Expand All @@ -120,7 +120,7 @@ void kalmanCoreUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurement_t
void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa);

// Measurements of flow (dnx, dny)
void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, flowMeasurement_t *flow, sensorData_t *sensors);
void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro);

// Measurements of TOF from laser sensor
void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof);
Expand All @@ -138,10 +138,10 @@ void kalmanCorePredict(kalmanCoreData_t* this, float thrust, Axis3f *acc, Axis3f
void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt);

/* - Finalization to incorporate attitude error into body attitude */
void kalmanCoreFinalize(kalmanCoreData_t* this, sensorData_t *sensors, uint32_t tick);
void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick);

/* - Externalization to move the filter's internal state into the external state expected by other modules */
void kalmanCoreExternalizeState(kalmanCoreData_t* this, state_t *state, sensorData_t *sensors, uint32_t tick);
void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc, uint32_t tick);

void kalmanCoreDecoupleXY(kalmanCoreData_t* this);

Expand Down
93 changes: 45 additions & 48 deletions src/modules/src/estimator_kalman.c
View file
Expand Up @@ -186,7 +186,6 @@ static inline bool stateEstimatorHasYawErrorPacket(float *error) {
* - X, Y, Z: the quad's position in the global frame
* - PX, PY, PZ: the quad's velocity in its body frame
* - D0, D1, D2: attitude error
* - SKEW: the skew from anchor system clock to quad clock
*
* For more information, refer to the paper
*/
Expand All @@ -204,7 +203,7 @@ static int32_t lastPNUpdate;
static Axis3f accAccumulator;
static float thrustAccumulator;
static Axis3f gyroAccumulator;
static baro_t baroAccumulator;
static float baroAslAccumulator;
static uint32_t accAccumulatorCount;
static uint32_t thrustAccumulatorCount;
static uint32_t gyroAccumulatorCount;
Expand All @@ -214,6 +213,12 @@ static int32_t lastTDOAUpdate;
static uint32_t lastFlightCmd;
static uint32_t takeoffTime;

#ifdef KALMAN_USE_BARO_UPDATE
static const bool useBaroUpdate = true;
#else
static const bool useBaroUpdate = false;
#endif

/**
* Supporting and utility functions
*/
Expand All @@ -231,7 +236,6 @@ static inline float arm_sqrt(float32_t in)

// --------------------------------------------------


void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick)
{
// If the client (via a parameter update) triggers an estimator reset:
Expand All @@ -243,7 +247,7 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
uint32_t osTick = xTaskGetTickCount(); // would be nice if this had a precision higher than 1ms...

#ifdef KALMAN_DECOUPLE_XY
kalmanCoreDecoupleXY(this);
kalmanCoreDecoupleXY(&coreData);
#endif

// Average the last IMU measurements. We do this because the prediction loop is
Expand Down Expand Up @@ -274,31 +278,31 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
&& thrustAccumulatorCount > 0)
{
// gyro is in deg/sec but the estimator requires rad/sec
gyroAccumulator.x *= DEG_TO_RAD;
gyroAccumulator.y *= DEG_TO_RAD;
gyroAccumulator.z *= DEG_TO_RAD;

gyroAccumulator.x /= gyroAccumulatorCount;
gyroAccumulator.y /= gyroAccumulatorCount;
gyroAccumulator.z /= gyroAccumulatorCount;
Axis3f gyroAverage;
gyroAverage.x = gyroAccumulator.x * DEG_TO_RAD / gyroAccumulatorCount;
gyroAverage.y = gyroAccumulator.y * DEG_TO_RAD / gyroAccumulatorCount;
gyroAverage.z = gyroAccumulator.z * DEG_TO_RAD / gyroAccumulatorCount;

// accelerometer is in Gs but the estimator requires ms^-2
accAccumulator.x *= GRAVITY_MAGNITUDE;
accAccumulator.y *= GRAVITY_MAGNITUDE;
accAccumulator.z *= GRAVITY_MAGNITUDE;

accAccumulator.x /= accAccumulatorCount;
accAccumulator.y /= accAccumulatorCount;
accAccumulator.z /= accAccumulatorCount;
Axis3f accAverage;
accAverage.x = accAccumulator.x * GRAVITY_MAGNITUDE / accAccumulatorCount;
accAverage.y = accAccumulator.y * GRAVITY_MAGNITUDE / accAccumulatorCount;
accAverage.z = accAccumulator.z * GRAVITY_MAGNITUDE / accAccumulatorCount;

// thrust is in grams, we need ms^-2
thrustAccumulator *= CONTROL_TO_ACC;
float thrustAverage = thrustAccumulator * CONTROL_TO_ACC / thrustAccumulatorCount;

accAccumulator = (Axis3f){.axis={0}};
accAccumulatorCount = 0;
gyroAccumulator = (Axis3f){.axis={0}};
gyroAccumulatorCount = 0;
thrustAccumulator = 0;
thrustAccumulatorCount = 0;

thrustAccumulator /= thrustAccumulatorCount;

// TODO: Find a better check for whether the quad is flying
// Assume that the flight begins when the thrust is large enough and for now we never stop "flying".
if (thrustAccumulator > IN_FLIGHT_THRUST_THRESHOLD) {
if (thrustAverage > IN_FLIGHT_THRUST_THRESHOLD) {
lastFlightCmd = xTaskGetTickCount();
if (!quadIsFlying) {
takeoffTime = lastFlightCmd;
Expand All @@ -307,17 +311,10 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
quadIsFlying = (xTaskGetTickCount()-lastFlightCmd) < IN_FLIGHT_TIME_THRESHOLD;

float dt = (float)(osTick-lastPrediction)/configTICK_RATE_HZ;
kalmanCorePredict(&coreData, thrustAccumulator, &accAccumulator, &gyroAccumulator, dt, quadIsFlying);
kalmanCorePredict(&coreData, thrustAverage, &accAverage, &gyroAverage, dt, quadIsFlying);

lastPrediction = osTick;

accAccumulator = (Axis3f){.axis={0}};
accAccumulatorCount = 0;
gyroAccumulator = (Axis3f){.axis={0}};
gyroAccumulatorCount = 0;
thrustAccumulator = 0;
thrustAccumulatorCount = 0;

doneUpdate = true;
}

Expand All @@ -334,24 +331,24 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
* Update the state estimate with the barometer measurements
*/
// Accumulate the barometer measurements
if (sensorsReadBaro(&sensors->baro)) {
#ifdef KALMAN_USE_BARO_UPDATE
baroAccumulator.asl += sensors->baro.asl;
baroAccumulatorCount++;
}
if (useBaroUpdate) {
if (sensorsReadBaro(&sensors->baro)) {
baroAslAccumulator += sensors->baro.asl;
baroAccumulatorCount++;
}

if ((osTick-lastBaroUpdate) >= configTICK_RATE_HZ/BARO_RATE // update at BARO_RATE
&& baroAccumulatorCount > 0)
{
baroAccumulator.asl /= baroAccumulatorCount;
if ((osTick-lastBaroUpdate) >= configTICK_RATE_HZ/BARO_RATE // update at BARO_RATE
&& baroAccumulatorCount > 0)
{
float baroAslAverage = baroAslAccumulator / baroAccumulatorCount;
baroAslAccumulator = 0;
baroAccumulatorCount = 0;

kalmanCoreUpdateWithBaro(&coreData, &sensors->baro, quadIsFlying);
kalmanCoreUpdateWithBaro(&coreData, baroAslAverage, quadIsFlying);

baroAccumulator.asl = 0;
baroAccumulatorCount = 0;
lastBaroUpdate = osTick;
doneUpdate = true;
#endif
lastBaroUpdate = osTick;
doneUpdate = true;
}
}

/**
Expand Down Expand Up @@ -411,7 +408,7 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
flowMeasurement_t flow;
while (stateEstimatorHasFlowPacket(&flow))
{
kalmanCoreUpdateWithFlow(&coreData, &flow, sensors);
kalmanCoreUpdateWithFlow(&coreData, &flow, &sensors->gyro);
doneUpdate = true;
}

Expand All @@ -424,7 +421,7 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,

if (doneUpdate)
{
kalmanCoreFinalize(&coreData, sensors, osTick);
kalmanCoreFinalize(&coreData, osTick);
if (! kalmanSupervisorIsStateWithinBounds(&coreData)) {
coreData.resetEstimation = true;
DEBUG_PRINT("State out of bounds, resetting\n");
Expand All @@ -435,7 +432,7 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
* Finally, the internal state is externalized.
* This is done every round, since the external state includes some sensor data
*/
kalmanCoreExternalizeState(&coreData, state, sensors, osTick);
kalmanCoreExternalizeState(&coreData, state, &sensors->acc, osTick);
}


Expand Down Expand Up @@ -469,7 +466,7 @@ void estimatorKalmanInit(void) {
accAccumulator = (Axis3f){.axis={0}};
gyroAccumulator = (Axis3f){.axis={0}};
thrustAccumulator = 0;
baroAccumulator.asl = 0;
baroAslAccumulator = 0;

accAccumulatorCount = 0;
gyroAccumulatorCount = 0;
Expand Down
26 changes: 13 additions & 13 deletions src/modules/src/kalman_core.c
View file
Expand Up @@ -90,7 +90,7 @@ static inline float arm_sqrt(float32_t in)


#ifdef DEBUG_STATE_CHECK
static void assertStateNotNaN(kalmanCoreData_t* this) {
static void assertStateNotNaN(const kalmanCoreData_t* this) {
if ((isnan(this->S[KC_STATE_X])) ||
(isnan(this->S[KC_STATE_Y])) ||
(isnan(this->S[KC_STATE_Z])) ||
Expand Down Expand Up @@ -119,7 +119,7 @@ static void assertStateNotNaN(kalmanCoreData_t* this) {
}
}
#else
static void assertStateNotNaN(kalmanCoreData_t* this)
static void assertStateNotNaN(const kalmanCoreData_t* this)
{
return;
}
Expand Down Expand Up @@ -288,7 +288,7 @@ static void scalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, fl
}


void kalmanCoreUpdateWithBaro(kalmanCoreData_t* this, baro_t *baro, bool quadIsFlying)
void kalmanCoreUpdateWithBaro(kalmanCoreData_t* this, float baroAsl, bool quadIsFlying)
{
float h[KC_STATE_DIM] = {0};
arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
Expand All @@ -297,10 +297,10 @@ void kalmanCoreUpdateWithBaro(kalmanCoreData_t* this, baro_t *baro, bool quadIsF

if (!quadIsFlying || this->baroReferenceHeight < 1) {
//TODO: maybe we could track the zero height as a state. Would be especially useful if UWB anchors had barometers.
this->baroReferenceHeight = baro->asl;
this->baroReferenceHeight = baroAsl;
}

float meas = (baro->asl - this->baroReferenceHeight);
float meas = (baroAsl - this->baroReferenceHeight);
scalarUpdate(this, &H, meas - this->S[KC_STATE_Z], measNoiseBaro);
}

Expand Down Expand Up @@ -461,7 +461,7 @@ static float predictedNY;
static float measuredNX;
static float measuredNY;

void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, flowMeasurement_t *flow, sensorData_t *sensors)
void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro)
{
// Inclusion of flow measurements in the EKF done by two scalar updates

Expand All @@ -472,8 +472,8 @@ void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, flowMeasurement_t *flow, s
float thetapix = DEG_TO_RAD * 4.2f;
//~~~ Body rates ~~~
// TODO check if this is feasible or if some filtering has to be done
float omegax_b = sensors->gyro.x * DEG_TO_RAD;
float omegay_b = sensors->gyro.y * DEG_TO_RAD;
float omegax_b = gyro->x * DEG_TO_RAD;
float omegay_b = gyro->y * DEG_TO_RAD;

// ~~~ Moves the body velocity into the global coordinate system ~~~
// [bar{x},bar{y},bar{z}]_G = R*[bar{x},bar{y},bar{z}]_B
Expand Down Expand Up @@ -839,7 +839,7 @@ void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt)



void kalmanCoreFinalize(kalmanCoreData_t* this, sensorData_t *sensors, uint32_t tick)
void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick)
{
// Matrix to rotate the attitude covariances once updated
static float A[KC_STATE_DIM][KC_STATE_DIM];
Expand Down Expand Up @@ -953,7 +953,7 @@ void kalmanCoreFinalize(kalmanCoreData_t* this, sensorData_t *sensors, uint32_t
assertStateNotNaN(this);
}

void kalmanCoreExternalizeState(kalmanCoreData_t* this, state_t *state, sensorData_t *sensors, uint32_t tick)
void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc, uint32_t tick)
{
// position state is already in world frame
state->position = (point_t){
Expand All @@ -976,9 +976,9 @@ void kalmanCoreExternalizeState(kalmanCoreData_t* this, state_t *state, sensorDa
// Finally, note that these accelerations are in Gs, and not in m/s^2, hence - 1 for removing gravity
state->acc = (acc_t){
.timestamp = tick,
.x = this->R[0][0]*sensors->acc.x + this->R[0][1]*sensors->acc.y + this->R[0][2]*sensors->acc.z,
.y = this->R[1][0]*sensors->acc.x + this->R[1][1]*sensors->acc.y + this->R[1][2]*sensors->acc.z,
.z = this->R[2][0]*sensors->acc.x + this->R[2][1]*sensors->acc.y + this->R[2][2]*sensors->acc.z - 1
.x = this->R[0][0]*acc->x + this->R[0][1]*acc->y + this->R[0][2]*acc->z,
.y = this->R[1][0]*acc->x + this->R[1][1]*acc->y + this->R[1][2]*acc->z,
.z = this->R[2][0]*acc->x + this->R[2][1]*acc->y + this->R[2][2]*acc->z - 1
};

// convert the new attitude into Euler YPR
Expand Down

0 comments on commit 30e9575

Please sign in to comment.