Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ThirdOrderCompFilter3D: first implementation of complementary filter for use with inertial navigation

This commit is contained in:
rmackay9 2012-11-05 13:30:34 +09:00
parent 48f1955f8e
commit dc7146c9ce
2 changed files with 246 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
libraries/Filter/ThirdOrderCompFilter3D.cpp Normal file
View File

@ -0,0 +1,157 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <FastSerial.h>
#include <ThirdOrderCompFilter3D.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include <wiring.h>
#endif
// Public Methods //////////////////////////////////////////////////////////////
// update_gains - update gains from time constant (given in seconds)
void ThirdOrderCompFilter3D::update_gains(float time_constant_seconds_xy, float time_constant_seconds_z)
{
static float last_time_constant_xy = 0;
static float last_time_constant_z = 0;
// X & Y axis time constant
if( time_constant_seconds_xy == 0 ) {
_k1_xy = _k2_xy = _k3_xy = 0;
}else{
if( time_constant_seconds_xy != last_time_constant_xy ) {
_k1_xy = 3 / time_constant_seconds_xy;
_k2_xy = 3 / (time_constant_seconds_xy*time_constant_seconds_xy);
_k3_xy = 1 / (time_constant_seconds_xy*time_constant_seconds_xy*time_constant_seconds_xy);
last_time_constant_xy = time_constant_seconds_xy;
}
}
// Z axis time constant
if( time_constant_seconds_z == 0 ) {
_k1_z = _k2_z = _k3_z = 0;
}else{
if( time_constant_seconds_z != last_time_constant_z ) {
_k1_z = 3 / time_constant_seconds_z;
_k2_z = 3 / (time_constant_seconds_z*time_constant_seconds_z);
_k3_z = 1 / (time_constant_seconds_z*time_constant_seconds_z*time_constant_seconds_z);
last_time_constant_z = time_constant_seconds_z;
}
}
}
// set_3rd_order - resets the first order value (i.e. position)
void ThirdOrderCompFilter3D::set_3rd_order_xy(float x, float y)
{
_comp_h.x = x;
_comp_h.y = y;
_comp_h_correction.x = 0;
_comp_h_correction.y = 0;
// clear historic estimates
_hist_3rd_order_estimates_x.clear();
_hist_3rd_order_estimates_y.clear();
}
// set_3rd_order - resets the first order value (i.e. position)
void ThirdOrderCompFilter3D::set_3rd_order_z(float z )
{
_comp_h.z = z;
_comp_h_correction.z = 0;
}
// set_2nd_order - resets the second order value (i.e. velocity)
void ThirdOrderCompFilter3D::set_2nd_order_xy(float x, float y)
{
_comp_v.x = x;
_comp_v.y = y;
}
// set_2nd_order - resets the second order value (i.e. velocity)
void ThirdOrderCompFilter3D::set_2nd_order_z(float z )
{
_comp_v.z = z;
}
// correct_3rd_order_z - correct accelerometer offsets using barometer or gps
void ThirdOrderCompFilter3D::correct_3rd_order_xy(float x, float y, Matrix3f& dcm_matrix, float deltat)
{
float hist_comp_h_x, hist_comp_h_y;
// 3rd order samples (i.e. position from gps) are delayed by 500ms
// we store historical position at 10hz so 5 iterations ago
if( _hist_3rd_order_estimates_x.num_items() >= 4 ) {
hist_comp_h_x = _hist_3rd_order_estimates_x.peek(3);
hist_comp_h_y = _hist_3rd_order_estimates_y.peek(3);
}else{
hist_comp_h_x = _comp_h.x;
hist_comp_h_y = _comp_h.y;
}
// calculate error in position from gps with our historical estimate
float err_x = x - (hist_comp_h_x + _comp_h_correction.x);
float err_y = y - (hist_comp_h_y + _comp_h_correction.y);
// calculate correction to accelerometers and apply in the body frame
_comp_k1o += dcm_matrix.mul_transpose(Vector3f((err_x*_k3_xy)*deltat,(err_y*_k3_xy)*deltat,0));
// correct velocity
_comp_v.x += (err_x*_k2_xy) * deltat;
_comp_v.y += (err_y*_k2_xy) * deltat;
// correct position
_comp_h_correction.x += err_x*_k1_xy * deltat;
_comp_h_correction.y += err_y*_k1_xy * deltat;
}
// correct_3rd_order_z - correct accelerometer offsets using barometer or gps
void ThirdOrderCompFilter3D::correct_3rd_order_z(float third_order_sample, Matrix3f& dcm_matrix, float deltat)
{
float hist_comp_h_z;
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
// so we should calculate error using historical estimates
if( _hist_3rd_order_estimates_z.num_items() >= 15 ) {
//hist_comp_h_z = _hist_3rd_order_estimates_z.get();
hist_comp_h_z = _hist_3rd_order_estimates_z.peek(14);
}else{
hist_comp_h_z = _comp_h.z;
}
// calculate error in position from baro with our estimate
float err = third_order_sample - (hist_comp_h_z + _comp_h_correction.z);
// calculate correction to accelerometers and apply in the body frame
_comp_k1o += dcm_matrix.mul_transpose(Vector3f(0,0,(err*_k3_z) * deltat));
// correct velocity
_comp_v.z += (err*_k2_z) * deltat;
// correct position
_comp_h_correction.z += err*_k1_z * deltat;
}
// recalculate the 2nd and 3rd order estimates
void ThirdOrderCompFilter3D::calculate(float deltat, Matrix3f& dcm_matrix)
{
// get earth frame accelerometer correction
comp_k1o_ef = dcm_matrix * _comp_k1o;
// calculate velocity by adding new acceleration from accelerometers
_comp_v += (-_first_order_sample + comp_k1o_ef) * deltat;
// calculate new estimate of position
_comp_h += _comp_v * deltat;
// store 3rd order estimate (i.e. estimated vertical position) for future use
_hist_3rd_order_estimates_z.add(_comp_h.z);
// store 3rd order estimate (i.e. horizontal position) for future use at 10hz
_historic_xy_counter++;
if( _historic_xy_counter >= THIRD_ORDER_SAVE_POS_10HZ ) {
_historic_xy_counter = 0;
_hist_3rd_order_estimates_x.add(_comp_h.x);
_hist_3rd_order_estimates_y.add(_comp_h.y);
}
}

89
libraries/Filter/ThirdOrderCompFilter3D.h Normal file
View File

@ -0,0 +1,89 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// This is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License as published by the
// Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.
//
/// @file ThirdOrderCompFilter3D.h
/// @brief A class to implement third order complementary filter (for combining barometer and GPS with accelerometer data)
/// math provided by Jonathan Challenger
#ifndef __THIRDORDERCOMPFILTER3D_H__
#define __THIRDORDERCOMPFILTER3D_H__
#include <inttypes.h>
#include <Math.h> // Math library for matrix and vector math
#include <AP_Buffer.h> // ArduPilot general purpose FIFO buffer
// #defines to control how often historical accel based positions are saved
// so they can later be compared to laggy gps readings
#define THIRD_ORDER_SAVE_POS_10HZ 10
#define THIRD_ORDER_SAVE_POS_5HZ 20
#define THIRD_ORDER_COMP_FILTER_HISTORIC_XY_SAVE_COUNTER_DEFAULT THIRD_ORDER_SAVE_POS_10HZ
class ThirdOrderCompFilter3D
{
public:
// constructor
ThirdOrderCompFilter3D(float time_constant_seconds_xy, float time_constant_seconds_z)
{
update_gains(time_constant_seconds_xy, time_constant_seconds_z);
};
// update_gains - update gains from time constant (given in seconds)
virtual void update_gains(float time_constant_seconds_xy, float time_constant_seconds_z);
// set_3rd_order - resets the first order value (i.e. position)
virtual void set_3rd_order_xy(float x, float y);
virtual void set_3rd_order_z(float z);
// set_2nd_order - resets the second order value (i.e. velocity)
virtual void set_2nd_order_xy(float x, float y);
virtual void set_2nd_order_z(float z);
// correct_3rd_order_z - correct accelerometer offsets using barometer or gps
virtual void correct_3rd_order_xy(float x, float y, Matrix3f& dcm_matrix, float deltat);
virtual void correct_3rd_order_z(float third_order_sample, Matrix3f& dcm_matrix, float deltat);
// add_1st_order_sample - Add a new 1st order sample (i.e. acceleration) to the filter, but don't recalculate
virtual void add_1st_order_sample(Vector3f& sample) { _first_order_sample = sample; }
// recalculate the 2nd and 3rd order estimates
virtual void calculate(float deltat, Matrix3f& dcm_matrix);
// return the new estimate for the 3rd order (i.e. position)
virtual Vector3f& get_3rd_order_estimate() { _comp_h_total = _comp_h + _comp_h_correction; return _comp_h_total; }
// return the new estimate for the 2nd order (i.e. velocity)
virtual Vector3f& get_2nd_order_estimate() { return _comp_v; }
// set the 1st order correction vector (i.e. correction to be applied to the accelerometer)
virtual void set_1st_order_correction( const Vector3f &correction_vector) { _comp_k1o = correction_vector; }
// get the 1st order correction vector (i.e. correction to be applied to the accelerometer)
virtual Vector3f& get_1st_order_correction( void ) { return _comp_k1o; }
//private:
float _k1_xy; // 1st order error correction gain for horizontal position
float _k2_xy; // 2nd order error correction gain for horizontal position
float _k3_xy; // 3rd order error correction gain for horizontal position
float _k1_z; // 1st order error correction gain for altitude
float _k2_z; // 2nd order error correction gain for altitude
float _k3_z; // 3rd order error correction gain for altitude
Vector3f _comp_k1o; // acceleration estimate
Vector3f _comp_v; // velocity estimate
Vector3f _comp_h; // position estimate
Vector3f _first_order_sample; // acceleration sample
uint8_t _historic_xy_counter; // historic positions saved when this counter reaches 10
AP_BufferFloat_Size10 _hist_3rd_order_estimates_x; // buffer of historic accel based position to account for lag
AP_BufferFloat_Size10 _hist_3rd_order_estimates_y; // buffer of historic accel based position to account for lag
AP_BufferFloat_Size15 _hist_3rd_order_estimates_z; // buffer of historic accel based altitudes to account for lag
Vector3f _comp_h_correction; // sum of correction to _comp_h from delayed 1st order samples
Vector3f _comp_h_total; // sum of _comp_h + _comp_h_correction
Vector3f comp_k1o_ef; // accelerometer correction in earth frame (only z element is used). here for debug purposes
};
#endif // __THIRDORDERCOMPFILTER3D_H__