flight/modules/StateEstimation/filtervelocity.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup State Estimation
   6  * @brief Acquires sensor data and computes state estimate
   7  * @{
   8  *
   9  * @file       filtervelocity.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Barometric altitude filter, calculates altitude offset based on
  12  *             GPS altitude offset if available
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  ******************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32
  33 #include "inc/stateestimation.h"
  34
  35 #include <revosettings.h>
  36
  37 // Private constants
  38
  39 #define STACK_REQUIRED 128
  40 #define DT_ALPHA       1e-3f
  41 #define DT_MIN         1e-6f
  42 #define DT_MAX         1.0f
  43 #define DT_INIT        (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate
  44
  45 // Private types
  46 struct data {
  47     float   vel[3];
  48     float   velocityBias[3];
  49     float   oldPos[3];
  50     float   alpha;
  51     uint8_t inited;
  52     PiOSDeltatimeConfig dtconfig;
  53 };
  54
  55 // Private variables
  56
  57 // Private functions
  58
  59 static int32_t init(stateFilter *self);
  60 static filterResult filter(stateFilter *self, stateEstimation *state);
  61
  62
  63 int32_t filterVelocityInitialize(stateFilter *handle)
  64 {
  65     handle->init      = &init;
  66     handle->filter    = &filter;
  67     handle->localdata = pios_malloc(sizeof(struct data));
  68     return STACK_REQUIRED;
  69 }
  70
  71 static int32_t init(stateFilter *self)
  72 {
  73     struct data *this = (struct data *)self->localdata;
  74
  75     this->vel[0] = 0.0f;
  76     this->vel[1] = 0.0f;
  77     this->vel[2] = 0.0f;
  78     this->velocityBias[0] = 0.0f;
  79     this->velocityBias[1] = 0.0f;
  80     this->velocityBias[2] = 0.0f;
  81     this->oldPos[0] = 0.0f;
  82     this->oldPos[1] = 0.0f;
  83     this->oldPos[2] = 0.0f;
  84     this->inited    = 0;
  85
  86     RevoSettingsInitialize();
  87     RevoSettingsVelocityPostProcessingLowPassAlphaGet(&this->alpha);
  88
  89     PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);
  90
  91     return 0;
  92 }
  93
  94 static filterResult filter(stateFilter *self, stateEstimation *state)
  95 {
  96     struct data *this = (struct data *)self->localdata;
  97
  98
  99     if (IS_SET(state->updated, SENSORUPDATES_pos)) {
 100         float dT;
 101         dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
 102
 103         // position updates allow us to calculate an ad-hoc velocity estimate
 104         // it will be very noisy and likely quite wrong at every given point in time,
 105         // but its long term average error should be quite low
 106
 107         if (this->inited >= 1) { // only calculate velocity estimate if previous position is known
 108             this->vel[0] = (state->pos[0] - this->oldPos[0]) / dT;
 109             this->vel[1] = (state->pos[1] - this->oldPos[1]) / dT;
 110             this->vel[2] = (state->pos[2] - this->oldPos[2]) / dT;
 111             this->inited = 2;
 112         } else {
 113             // mark previous position as known
 114             this->inited = 1;
 115         }
 116         this->oldPos[0] = state->pos[0];
 117         this->oldPos[1] = state->pos[1];
 118         this->oldPos[2] = state->pos[2];
 119     }
 120     if (IS_SET(state->updated, SENSORUPDATES_vel)) {
 121         // if we have both a velocity estimate from filter and a velocity estimate from postion
 122         // we can calculate a bias estimate. It must be heavily low pass filtered to become useful
 123         // this assumes that the bias is relatively constant over time
 124         if (this->inited >= 2) { // only calculate bias if velocity estimate is calculated
 125             this->velocityBias[0] *= this->alpha;
 126             this->velocityBias[0] += (1.0f - this->alpha) * (this->vel[0] - state->vel[0]);
 127             this->velocityBias[1] *= this->alpha;
 128             this->velocityBias[1] += (1.0f - this->alpha) * (this->vel[1] - state->vel[1]);
 129             this->velocityBias[2] *= this->alpha;
 130             this->velocityBias[2] += (1.0f - this->alpha) * (this->vel[2] - state->vel[2]);
 131         }
 132         state->vel[0] += this->velocityBias[0];
 133         state->vel[1] += this->velocityBias[1];
 134         state->vel[2] += this->velocityBias[2];
 135     }
 136     return FILTERRESULT_OK;
 137 }
 138
 139
 140 /**
 141  * @}
 142  * @}
 143  */