mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-30 15:52:12 +01:00
added filter for magnetometer bias drift compensation
This commit is contained in:
parent
1bd9cdafab
commit
2f082dbf87
182
flight/modules/StateEstimation/filtermag.c
Normal file
182
flight/modules/StateEstimation/filtermag.c
Normal file
@ -0,0 +1,182 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup State Estimation
|
||||
* @brief Acquires sensor data and computes state estimate
|
||||
* @{
|
||||
*
|
||||
* @file filtermag.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
||||
* @brief Magnetometer drift compensation, uses previous cycles
|
||||
* AttitudeState for estimation
|
||||
*
|
||||
* @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 <attitudestate.h>
|
||||
#include <revocalibration.h>
|
||||
#include <homelocation.h>
|
||||
|
||||
#include <CoordinateConversions.h>
|
||||
|
||||
// Private constants
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
|
||||
static HomeLocationData homeLocation;
|
||||
static RevoCalibrationData revoCalibration;
|
||||
static float magBias[3] = { 0 };
|
||||
|
||||
// Private functions
|
||||
|
||||
static int32_t init(void);
|
||||
static int32_t filter(stateEstimation *state);
|
||||
static void magOffsetEstimation(float mag[3]);
|
||||
|
||||
|
||||
void filterMagInitialize(stateFilter *handle)
|
||||
{
|
||||
handle->init = &init;
|
||||
handle->filter = &filter;
|
||||
}
|
||||
|
||||
static int32_t init(void)
|
||||
{
|
||||
magBias[0] = magBias[1] = magBias[2] = 0.0f;
|
||||
HomeLocationGet(&homeLocation);
|
||||
RevoCalibrationGet(&revoCalibration);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int32_t filter(stateEstimation *state)
|
||||
{
|
||||
if (ISSET(state->updated, mag_UPDATED)) {
|
||||
if (revoCalibration.MagBiasNullingRate > 0) {
|
||||
magOffsetEstimation(state->mag);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform an update of the @ref MagBias based on
|
||||
* Magmeter Offset Cancellation: Theory and Implementation,
|
||||
* revisited William Premerlani, October 14, 2011
|
||||
*/
|
||||
static void magOffsetEstimation(float mag[3])
|
||||
{
|
||||
#if 0
|
||||
// Constants, to possibly go into a UAVO
|
||||
static const float MIN_NORM_DIFFERENCE = 50;
|
||||
|
||||
static float B2[3] = { 0, 0, 0 };
|
||||
|
||||
MagBiasData magBias;
|
||||
MagBiasGet(&magBias);
|
||||
|
||||
// Remove the current estimate of the bias
|
||||
mag->x -= magBias.x;
|
||||
mag->y -= magBias.y;
|
||||
mag->z -= magBias.z;
|
||||
|
||||
// First call
|
||||
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
|
||||
B2[0] = mag->x;
|
||||
B2[1] = mag->y;
|
||||
B2[2] = mag->z;
|
||||
return;
|
||||
}
|
||||
|
||||
float B1[3] = { mag->x, mag->y, mag->z };
|
||||
float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
|
||||
if (norm_diff > MIN_NORM_DIFFERENCE) {
|
||||
float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
|
||||
float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
|
||||
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
|
||||
float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
|
||||
|
||||
magBias.x += b_error[0];
|
||||
magBias.y += b_error[1];
|
||||
magBias.z += b_error[2];
|
||||
|
||||
MagBiasSet(&magBias);
|
||||
|
||||
// Store this value to compare against next update
|
||||
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
|
||||
}
|
||||
#else // if 0
|
||||
|
||||
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]);
|
||||
const float Rz = homeLocation.Be[2];
|
||||
|
||||
const float rate = revoCalibration.MagBiasNullingRate;
|
||||
float Rot[3][3];
|
||||
float B_e[3];
|
||||
float xy[2];
|
||||
float delta[3];
|
||||
|
||||
AttitudeStateData attitude;
|
||||
AttitudeStateGet(&attitude);
|
||||
|
||||
// Get the rotation matrix
|
||||
Quaternion2R(&attitude.q1, Rot);
|
||||
|
||||
// Rotate the mag into the NED frame
|
||||
B_e[0] = Rot[0][0] * mag[0] + Rot[1][0] * mag[1] + Rot[2][0] * mag[2];
|
||||
B_e[1] = Rot[0][1] * mag[0] + Rot[1][1] * mag[1] + Rot[2][1] * mag[2];
|
||||
B_e[2] = Rot[0][2] * mag[0] + Rot[1][2] * mag[1] + Rot[2][2] * mag[2];
|
||||
|
||||
float cy = cosf(DEG2RAD(attitude.Yaw));
|
||||
float sy = sinf(DEG2RAD(attitude.Yaw));
|
||||
|
||||
xy[0] = cy * B_e[0] + sy * B_e[1];
|
||||
xy[1] = -sy * B_e[0] + cy * B_e[1];
|
||||
|
||||
float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
|
||||
|
||||
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
|
||||
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
|
||||
delta[2] = -rate * (Rz - B_e[2]);
|
||||
|
||||
if (!isnan(delta[0]) && !isinf(delta[0]) &&
|
||||
!isnan(delta[1]) && !isinf(delta[1]) &&
|
||||
!isnan(delta[2]) && !isinf(delta[2])) {
|
||||
magBias[0] += delta[0];
|
||||
magBias[1] += delta[1];
|
||||
magBias[2] += delta[2];
|
||||
}
|
||||
|
||||
// Add bias to state estimation
|
||||
mag[0] += magBias[0];
|
||||
mag[1] += magBias[1];
|
||||
mag[2] += magBias[2];
|
||||
|
||||
#endif // if 0
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
Loading…
x
Reference in New Issue
Block a user