mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-22 14:19:42 +01:00
Complementary filter added
This commit is contained in:
Corvus Corax
parent
741c70cda4
commit
a03f87efb5
410
flight/modules/StateEstimation/filtercf.c
Normal file
410
flight/modules/StateEstimation/filtercf.c
Normal file
@ -0,0 +1,410 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup State Estimation
|
||||
* @brief Acquires sensor data and computes state estimate
|
||||
* @{
|
||||
*
|
||||
* @file filtercf.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
||||
* @brief Complementary filter to calculate Attitude from Accels and Gyros
|
||||
* and optionally magnetometers:
|
||||
* WARNING: Will drift if the mean acceleration force doesn't point
|
||||
* to ground, unsafe on fixed wing, since position hold is
|
||||
* implemented as continuous circular flying!
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
******************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "inc/stateestimation.h"
|
||||
|
||||
#include <attitudesettings.h>
|
||||
#include <attitudestate.h>
|
||||
#include <flightstatus.h>
|
||||
#include <homelocation.h>
|
||||
|
||||
#include <CoordinateConversions.h>
|
||||
|
||||
// Private constants
|
||||
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static AttitudeSettingsData attitudeSettings;
|
||||
static FlightStatusData flightStatus;
|
||||
static HomeLocationData homeLocation;
|
||||
|
||||
static bool first_run = 1;
|
||||
static bool useMag = 0;
|
||||
static float currentAccel[3];
|
||||
static float currentMag[3];
|
||||
static float gyroBias[3];
|
||||
static bool accelUpdated = 0;
|
||||
static bool magUpdated = 0;
|
||||
|
||||
|
||||
static float accel_alpha = 0;
|
||||
static bool accel_filter_enabled = false;
|
||||
|
||||
// Private functions
|
||||
|
||||
static int32_t initwithmag(void);
|
||||
static int32_t initwithoutmag(void);
|
||||
static int32_t maininit(void);
|
||||
static int32_t filter(stateEstimation *state);
|
||||
static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4]);
|
||||
|
||||
static void flightStatusUpdatedCb(UAVObjEvent *ev);
|
||||
|
||||
|
||||
void filterCFInitialize(stateFilter *handle)
|
||||
{
|
||||
handle->init = &initwithoutmag;
|
||||
handle->filter = &filter;
|
||||
FlightStatusConnectCallback(&flightStatusUpdatedCb);
|
||||
HomeLocationConnectCallback(&flightStatusUpdatedCb);
|
||||
flightStatusUpdatedCb(NULL);
|
||||
}
|
||||
|
||||
void filterCFMInitialize(stateFilter *handle)
|
||||
{
|
||||
handle->init = &initwithmag;
|
||||
handle->filter = &filter;
|
||||
}
|
||||
|
||||
static int32_t initwithmag(void)
|
||||
{
|
||||
useMag = 1;
|
||||
return maininit();
|
||||
}
|
||||
|
||||
static int32_t initwithoutmag(void)
|
||||
{
|
||||
useMag = 0;
|
||||
return maininit();
|
||||
}
|
||||
|
||||
static int32_t maininit(void)
|
||||
{
|
||||
first_run = 1;
|
||||
accelUpdated = 0;
|
||||
AttitudeSettingsGet(&attitudeSettings);
|
||||
|
||||
const float fakeDt = 0.0025f;
|
||||
if (attitudeSettings.AccelTau < 0.0001f) {
|
||||
accel_alpha = 0; // not trusting this to resolve to 0
|
||||
accel_filter_enabled = false;
|
||||
} else {
|
||||
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau);
|
||||
accel_filter_enabled = true;
|
||||
}
|
||||
|
||||
// reset gyro Bias
|
||||
gyroBias[0] = 0.0f;
|
||||
gyroBias[1] = 0.0f;
|
||||
gyroBias[2] = 0.0f;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Collect all required state variables, then run complementary filter
|
||||
*/
|
||||
static int32_t filter(stateEstimation *state)
|
||||
{
|
||||
int32_t result = 0;
|
||||
|
||||
if (ISSET(state->updated, mag_UPDATED)) {
|
||||
magUpdated = 1;
|
||||
currentMag[0] = state->mag[0];
|
||||
currentMag[1] = state->mag[1];
|
||||
currentMag[2] = state->mag[2];
|
||||
}
|
||||
if (ISSET(state->updated, acc_UPDATED)) {
|
||||
accelUpdated = 1;
|
||||
currentAccel[0] = state->acc[0];
|
||||
currentAccel[1] = state->acc[1];
|
||||
currentAccel[2] = state->acc[2];
|
||||
}
|
||||
if (ISSET(state->updated, gyr_UPDATED)) {
|
||||
if (accelUpdated) {
|
||||
float att[4];
|
||||
result = complementaryFilter(state->gyr, currentAccel, currentMag, att);
|
||||
if (!result) {
|
||||
state->att[0] = att[0];
|
||||
state->att[1] = att[1];
|
||||
state->att[2] = att[2];
|
||||
state->att[3] = att[3];
|
||||
state->updated |= att_UPDATED;
|
||||
}
|
||||
accelUpdated = 0;
|
||||
magUpdated = 0;
|
||||
}
|
||||
state->gyr[0] -= gyroBias[0];
|
||||
state->gyr[1] -= gyroBias[1];
|
||||
state->gyr[2] -= gyroBias[2];
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
static inline void apply_accel_filter(const float *raw, float *filtered)
|
||||
{
|
||||
if (accel_filter_enabled) {
|
||||
filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha);
|
||||
filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha);
|
||||
filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha);
|
||||
} else {
|
||||
filtered[0] = raw[0];
|
||||
filtered[1] = raw[1];
|
||||
filtered[2] = raw[2];
|
||||
}
|
||||
}
|
||||
|
||||
static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4])
|
||||
{
|
||||
static int32_t timeval;
|
||||
float dT;
|
||||
static uint8_t init = 0;
|
||||
float magKp = 0.0f; // TODO: make this non hardcoded at some point
|
||||
float magKi = 0.000001f;
|
||||
|
||||
// During initialization and
|
||||
if (first_run) {
|
||||
#if defined(PIOS_INCLUDE_HMC5883)
|
||||
// wait until mags have been updated
|
||||
if (!mag_updated) {
|
||||
return 1;
|
||||
}
|
||||
#else
|
||||
mag[0] = 100.0f;
|
||||
mag[1] = 0.0f;
|
||||
mag[2] = 0.0f;
|
||||
#endif
|
||||
AttitudeStateData attitudeState; // base on previous state
|
||||
AttitudeStateGet(&attitudeState);
|
||||
init = 0;
|
||||
|
||||
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
|
||||
// so pseudo "north" vector can be estimated even if the board is not level
|
||||
attitudeState.Roll = atan2f(-accel[2], -accel[3]);
|
||||
float zn = cosf(attitudeState.Roll) * mag[2] + sinf(attitudeState.Roll) * mag[1];
|
||||
float yn = cosf(attitudeState.Roll) * mag[1] - sinf(attitudeState.Roll) * mag[2];
|
||||
|
||||
// rotate accels z vector according to roll
|
||||
float azn = cosf(attitudeState.Roll) * accel[2] + sinf(attitudeState.Roll) * accel[1];
|
||||
attitudeState.Pitch = atan2f(accel[0], -azn);
|
||||
|
||||
float xn = cosf(attitudeState.Pitch) * mag[0] + sinf(attitudeState.Pitch) * zn;
|
||||
|
||||
attitudeState.Yaw = atan2f(-yn, xn);
|
||||
// TODO: This is still a hack
|
||||
// Put this in a proper generic function in CoordinateConversion.c
|
||||
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
|
||||
// should calculate the rotation in 3d space using proper cross product math
|
||||
// SUBTODO: formulate the math required
|
||||
|
||||
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
|
||||
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
|
||||
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
|
||||
|
||||
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
|
||||
q[0] = attitudeState.q1;
|
||||
q[1] = attitudeState.q1;
|
||||
q[2] = attitudeState.q1;
|
||||
q[3] = attitudeState.q1;
|
||||
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
|
||||
// For first 7 seconds use accels to get gyro bias
|
||||
attitudeSettings.AccelKp = 1.0f;
|
||||
attitudeSettings.AccelKi = 0.9f;
|
||||
attitudeSettings.YawBiasRate = 0.23f;
|
||||
magKp = 1.0f;
|
||||
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
|
||||
attitudeSettings.AccelKp = 1.0f;
|
||||
attitudeSettings.AccelKi = 0.9f;
|
||||
attitudeSettings.YawBiasRate = 0.23f;
|
||||
magKp = 1.0f;
|
||||
init = 0;
|
||||
} else if (init == 0) {
|
||||
// Reload settings (all the rates)
|
||||
AttitudeSettingsGet(&attitudeSettings);
|
||||
magKp = 0.01f;
|
||||
init = 1;
|
||||
}
|
||||
|
||||
// Compute the dT using the cpu clock
|
||||
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
if (dT < 0.001f) { // safe bounds
|
||||
dT = 0.001f;
|
||||
}
|
||||
|
||||
AttitudeStateData attitudeState; // base on previous state
|
||||
AttitudeStateGet(&attitudeState);
|
||||
|
||||
// Get the current attitude estimate
|
||||
quat_copy(&attitudeState.q1, q);
|
||||
|
||||
float accels_filtered[3];
|
||||
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
|
||||
apply_accel_filter(accel, accels_filtered);
|
||||
|
||||
// Rotate gravity to body frame and cross with accels
|
||||
float grot[3];
|
||||
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
|
||||
grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
|
||||
grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
|
||||
|
||||
float grot_filtered[3];
|
||||
float accel_err[3];
|
||||
apply_accel_filter(grot, grot_filtered);
|
||||
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
|
||||
|
||||
// Account for accel magnitude
|
||||
float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
|
||||
if (accel_mag < 1.0e-3f) {
|
||||
return 2; // safety feature copied from CC
|
||||
}
|
||||
|
||||
// Account for filtered gravity vector magnitude
|
||||
float grot_mag;
|
||||
if (accel_filter_enabled) {
|
||||
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
|
||||
} else {
|
||||
grot_mag = 1.0f;
|
||||
}
|
||||
if (grot_mag < 1.0e-3f) {
|
||||
return 2;
|
||||
}
|
||||
|
||||
accel_err[0] /= (accel_mag * grot_mag);
|
||||
accel_err[1] /= (accel_mag * grot_mag);
|
||||
accel_err[2] /= (accel_mag * grot_mag);
|
||||
|
||||
float mag_err[3] = { 0.0f };
|
||||
if (magUpdated && useMag) {
|
||||
// Rotate gravity to body frame and cross with accels
|
||||
float brot[3];
|
||||
float Rbe[3][3];
|
||||
|
||||
Quaternion2R(q, Rbe);
|
||||
|
||||
rot_mult(Rbe, homeLocation.Be, brot);
|
||||
|
||||
float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
|
||||
mag[0] /= mag_len;
|
||||
mag[1] /= mag_len;
|
||||
mag[2] /= mag_len;
|
||||
|
||||
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
|
||||
brot[0] /= bmag;
|
||||
brot[1] /= bmag;
|
||||
brot[2] /= bmag;
|
||||
|
||||
// Only compute if neither vector is null
|
||||
if (bmag < 1.0f || mag_len < 1.0f) {
|
||||
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
|
||||
} else {
|
||||
CrossProduct((const float *)mag, (const float *)brot, mag_err);
|
||||
}
|
||||
} else {
|
||||
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
|
||||
}
|
||||
|
||||
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
|
||||
gyroBias[0] -= accel_err[0] * attitudeSettings.AccelKi;
|
||||
gyroBias[1] -= accel_err[1] * attitudeSettings.AccelKi;
|
||||
if (useMag) {
|
||||
gyroBias[2] -= mag_err[2] * magKi;
|
||||
}
|
||||
|
||||
// Correct rates based on integral coefficient
|
||||
gyro[0] -= gyroBias[0];
|
||||
gyro[1] -= gyroBias[1];
|
||||
gyro[2] -= gyroBias[2];
|
||||
|
||||
float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
|
||||
// Correct rates based on proportional coefficient
|
||||
gyrotmp[0] += accel_err[0] * attitudeSettings.AccelKp / dT;
|
||||
gyrotmp[1] += accel_err[1] * attitudeSettings.AccelKp / dT;
|
||||
if (useMag) {
|
||||
gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
|
||||
} else {
|
||||
gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT;
|
||||
}
|
||||
|
||||
// Work out time derivative from INSAlgo writeup
|
||||
// Also accounts for the fact that gyros are in deg/s
|
||||
float qdot[4];
|
||||
qdot[0] = DEG2RAD(-q[1] * gyrotmp[0] - q[2] * gyrotmp[1] - q[3] * gyrotmp[2]) * dT / 2;
|
||||
qdot[1] = DEG2RAD(q[0] * gyrotmp[0] - q[3] * gyrotmp[1] + q[2] * gyrotmp[2]) * dT / 2;
|
||||
qdot[2] = DEG2RAD(q[3] * gyrotmp[0] + q[0] * gyrotmp[1] - q[1] * gyrotmp[2]) * dT / 2;
|
||||
qdot[3] = DEG2RAD(-q[2] * gyrotmp[0] + q[1] * gyrotmp[1] + q[0] * gyrotmp[2]) * dT / 2;
|
||||
|
||||
// Take a time step
|
||||
q[0] = q[0] + qdot[0];
|
||||
q[1] = q[1] + qdot[1];
|
||||
q[2] = q[2] + qdot[2];
|
||||
q[3] = q[3] + qdot[3];
|
||||
|
||||
if (q[0] < 0.0f) {
|
||||
q[0] = -q[0];
|
||||
q[1] = -q[1];
|
||||
q[2] = -q[2];
|
||||
q[3] = -q[3];
|
||||
}
|
||||
|
||||
// Renomalize
|
||||
float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
|
||||
q[0] = q[0] / qmag;
|
||||
q[1] = q[1] / qmag;
|
||||
q[2] = q[2] / qmag;
|
||||
q[3] = q[3] / qmag;
|
||||
|
||||
// If quaternion has become inappropriately short or is nan reinit.
|
||||
// THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
|
||||
q[0] = 1.0f;
|
||||
q[1] = 0.0f;
|
||||
q[2] = 0.0f;
|
||||
q[3] = 0.0f;
|
||||
return 2;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void flightStatusUpdatedCb(UAVObjEvent *ev)
|
||||
{
|
||||
FlightStatusGet(&flightStatus);
|
||||
HomeLocationGet(&homeLocation);
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -355,16 +355,21 @@ static void StateEstimationCb(void)
|
||||
|
||||
// apply all filters in the current filter chain
|
||||
filterQueue *current = (filterQueue *)filterChain;
|
||||
bool error = 0;
|
||||
uint8_t error = 0;
|
||||
while (current != NULL) {
|
||||
int32_t result = current->filter->filter(&states);
|
||||
if (result != 0) {
|
||||
error = 1;
|
||||
if (result > error) {
|
||||
error = result;
|
||||
alarm = 1;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
if (error) {
|
||||
if (error == 1) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else if (error == 2) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else if (error == 3) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
|
||||
// the final output of filters is saved in state variables
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user