1  // %flair:license{


2  // This file is part of the Flair framework distributed under the


3  // CECILLC License, Version 1.0.


4  // %flair:license}


5  // created: 2007/10/17, 2014/01/15


6  // filename: AhrsKalman_impl.cpp


7  //


8  // author: Tom Pycke (tom.pycke@gmail.com)


9  // API changes by Guillaume Sanahuja to fit the Flair framework


10  //


11  // version: $Id: $


12  //


13  // purpose: Class defining an Ahrs Kalman filter


14  //


15  //


16  /*********************************************************************/


17 


18  /***********************************************************************


19  * *


20  * This file contains the code for the kalman filter that uses the *


21  * sensor data as inputs. *


22  * *


23  * *


24  ***********************************************************************


25  * *


26  * Comments: *


27  * To help others to understand the kalman filter, I used one of *


28  * the most accessible sources with information on it: *


29  * http://en.wikipedia.org/wiki/Kalman_filter *


30  * The code is split into 2 parts: a Predict function and an *


31  * Update function, just as the wikipedia page does. *


32  * The model I used in the kalman filter is the following: *


33  * Our gyroscope measures the turnrate in degrees per second. This *


34  * is the derivative of the angle, called dotAngle. The bias is the *


35  * output of our gyro when the rotationrate is 0 degrees per second *


36  * (not rotating). Because of drift it changes over time. *


37  * So mathematically we just integrate the gyro (dt is timespan *


38  * since last integration): *


39  * angle = angle + (dotAngle  bias) * dt *


40  * When we include the bias in our model, the kalman filter will *


41  * try to estimate the bias, thanks to our last input: the measured *


42  * angle. This is just an estimate and comes from the accelerometer. *


43  * So the state used in our filter had 2 dimensions: [ angle, bias ] *


44  * Jump to the functions to read more about the actual *


45  * implementation. *


46  * *


47  ***********************************************************************/


48 


49  #include "AhrsKalman.h"


50  #include "AhrsKalman_impl.h"


51  #include <Imu.h>


52  #include <GroupBox.h>


53  #include <DoubleSpinBox.h>


54  #include <Layout.h>


55  #include <LayoutPosition.h>


56  #include <ImuData.h>


57  #include <AhrsData.h>


58  #include <math.h>


59 


60  #define PI ((float)3.14159265358979323846)


61 


62  using std::string;


63  using namespace flair::core;


64  using namespace flair::gui;


65 


66  AhrsKalman_impl::AhrsKalman_impl(const Layout* layout,AhrsData *inAhrsData): ahrsData(inAhrsData) {


67  euler.yaw=0;


68 


69  Q_angle=new DoubleSpinBox(layout>NewRow(),"Q_angle:",0.,10000,0.0001,4);


70  Q_gyro=new DoubleSpinBox(layout>NewRow(),"Q_gyro:",0.,10000,0.0001,4);


71  R_angle=new DoubleSpinBox(layout>NewRow(),"R_angle:",0.,1000000,0.0001,4);


72 


73 


74  //init ars


75  //ars_Init(&ars_roll, 1/*Q_angle*/, 0.1/*Q_gyro*/, 10000/*R_angle*/);


76  //ars_Init(&ars_pitch, 1/*Q_angle*/, 0.1/*Q_gyro*/, 10000/*R_angle*/);


77  ars_Init(&ars_roll, Q_angle>Value(), Q_gyro>Value(), R_angle>Value());


78  ars_Init(&ars_pitch, Q_angle>Value(), Q_gyro>Value(), R_angle>Value());


79  }


80 


81  AhrsKalman_impl::~AhrsKalman_impl() {


82  }


83 


84  void AhrsKalman_impl::UpdateFrom(const io_data *data) {


85  ImuData *input=(ImuData*)data;


86  float delta_t;


87  Vector3Df rawAcc,rawMag,rawGyr;


88 


89  input>GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);


90 


91  delta_t=(float)(data>DataDeltaTime())/1000000000.;


92 


93  if(Q_angle>ValueChanged()  Q_gyro>ValueChanged()  R_angle>ValueChanged()) {


94  ars_Init(&ars_roll, Q_angle>Value(), Q_gyro>Value(), R_angle>Value());


95  ars_Init(&ars_pitch, Q_angle>Value(), Q_gyro>Value(), R_angle>Value());


96  }


97 


98  if(delta_t!=0) {


99  //execute kalman roll filter


100  ars_predict(&ars_roll, rawGyr.x, delta_t);


101  ars_update(&ars_roll, atan2(rawAcc.y, rawAcc.z));//1 ou az


102 


103  //execute kalman pitch filter


104  ars_predict(&ars_pitch, rawGyr.y, delta_t);


105  ars_update(&ars_pitch, atan2(rawAcc.x, rawAcc.z));//1 ou az


106 


107 


108  euler.roll=ars_roll.x_angle;


109  euler.pitch=ars_pitch.x_angle;


110  euler.yaw += rawGyr.z *delta_t;


111  if(euler.yaw>PI) euler.yaw=2*PI;


112  if(euler.yaw<PI) euler.yaw+=2*PI;


113 


114  ahrsData>SetQuaternionAndAngularRates(euler.ToQuaternion(),


115  rawGyrVector3Df(ars_roll.x_bias,ars_pitch.x_bias,0));


116  }


117  ahrsData>SetDataTime(data>DataTime());


118  }


119 


120 


121  void AhrsKalman_impl::ars_Init(ars_Gyro1DKalman *ars, double Q_angle, double Q_gyro, double R_angle) {


122  ars>Q_angle = Q_angle;


123  ars>Q_gyro = Q_gyro;


124  ars>R_angle = R_angle;


125  ars>x_bias=0;


126  ars>x_angle=0;


127  ars>P_00 =0;


128  ars>P_01 =0;


129  ars>P_10 =0;


130  ars>P_11 =0;


131  }


132 


133  /*


134  * The predict function. Updates 2 variables:


135  * our modelstate x and the 2x2 matrix P


136  *


137  * x = [ angle, bias ]'


138  *


139  * = F x + B u


140  *


141  * = [ 1 dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]


142  *


143  * => angle = angle + dt (dotAngle  bias)


144  * bias = bias


145  *


146  *


147  * P = F P transpose(F) + Q


148  *


149  * = [ 1 dt, 0 1 ] * P * [ 1 0, dt 1 ] + Q


150  *


151  * P(0,0) = P(0,0)  dt * ( P(1,0) + P(0,1) ) + dt * P(1,1) + Q(0,0)


152  * P(0,1) = P(0,1)  dt * P(1,1) + Q(0,1)


153  * P(1,0) = P(1,0)  dt * P(1,1) + Q(1,0)


154  * P(1,1) = P(1,1) + Q(1,1)


155  *


156  *


157  */


158  void AhrsKalman_impl::ars_predict(ars_Gyro1DKalman *ars, const double dotAngle, const double dt) {


159  ars>x_angle += dt * (dotAngle  ars>x_bias);


160 


161  ars>P_00 +=  dt * (ars>P_10 + ars>P_01) + ars>Q_angle * dt;


162  ars>P_01 +=  dt * ars>P_11;


163  ars>P_10 +=  dt * ars>P_11;


164  ars>P_11 += + ars>Q_gyro * dt;


165  }


166 


167  /*


168  * The update function updates our model using


169  * the information from a 2nd measurement.


170  * Input angle_m is the angle measured by the accelerometer.


171  *


172  * y = z  H x


173  *


174  * S = H P transpose(H) + R


175  * = [ 1 0 ] P [ 1, 0 ] + R


176  * = P(0,0) + R


177  *


178  * K = P transpose(H) S^1


179  * = [ P(0,0), P(1,0) ] / S


180  *


181  * x = x + K y


182  *


183  * P = (I  K H) P


184  *


185  * = ( [ 1 0, [ K(0),


186  * 0 1 ]  K(1) ] * [ 1 0 ] ) P


187  *


188  * = [ P(0,0)P(0,0)*K(0) P(0,1)P(0,1)*K(0),


189  * P(1,0)P(0,0)*K(1) P(1,1)P(0,1)*K(1) ]


190  */


191  void AhrsKalman_impl::ars_update(ars_Gyro1DKalman *ars, const double angle_m) {


192  const double y = angle_m  ars>x_angle;


193 


194  const double S = ars>P_00 + ars>R_angle;


195  const double K_0 = ars>P_00 / S;


196  const double K_1 = ars>P_10 / S;


197 


198  ars>x_angle += K_0 * y;


199  ars>x_bias += K_1 * y;


200 


201  ars>P_00 = K_0 * ars>P_00;


202  ars>P_01 = K_0 * ars>P_01;


203  ars>P_10 = K_1 * ars>P_00;


204  ars>P_11 = K_1 * ars>P_01;


205  }

