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  is_init=false;


68  euler.yaw=0;


69 


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


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


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


73 


74 


75  //init ars


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


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


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


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


80  }


81 


82  AhrsKalman_impl::~AhrsKalman_impl() {


83  }


84 


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


86  ImuData *input=(ImuData*)data;


87  float delta_t;


88  Vector3Df rawAcc,rawMag,rawGyr;


89 


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


91 


92  delta_t=(float)(data>DataTime()previous_time)/1000000000.;


93  previous_time=data>DataTime();


94 


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


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


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


98  }


99 


100  if(is_init==true) {


101  //execute kalman roll filter


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


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


104 


105  //execute kalman pitch filter


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


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


108 


109 


110  euler.roll=ars_roll.x_angle;


111  euler.pitch=ars_pitch.x_angle;


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


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


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


115 


116  ahrsData>SetQuaternionAndAngularRates(euler.ToQuaternion(),


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


118  } else {


119  is_init=true;


120  }


121 


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


123  }


124 


125 


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


127  ars>Q_angle = Q_angle;


128  ars>Q_gyro = Q_gyro;


129  ars>R_angle = R_angle;


130  ars>x_bias=0;


131  ars>x_angle=0;


132  ars>P_00 =0;


133  ars>P_01 =0;


134  ars>P_10 =0;


135  ars>P_11 =0;


136  }


137 


138  /*


139  * The predict function. Updates 2 variables:


140  * our modelstate x and the 2x2 matrix P


141  *


142  * x = [ angle, bias ]'


143  *


144  * = F x + B u


145  *


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


147  *


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


149  * bias = bias


150  *


151  *


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


153  *


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


155  *


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


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


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


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


160  *


161  *


162  */


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


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


165 


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


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


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


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


170  }


171 


172  /*


173  * The update function updates our model using


174  * the information from a 2nd measurement.


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


176  *


177  * y = z  H x


178  *


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


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


181  * = P(0,0) + R


182  *


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


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


185  *


186  * x = x + K y


187  *


188  * P = (I  K H) P


189  *


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


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


192  *


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


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


195  */


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


197  const double y = angle_m  ars>x_angle;


198 


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


200  const double K_0 = ars>P_00 / S;


201  const double K_1 = ars>P_10 / S;


202 


203  ars>x_angle += K_0 * y;


204  ars>x_bias += K_1 * y;


205 


206  ars>P_00 = K_0 * ars>P_00;


207  ars>P_01 = K_0 * ars>P_01;


208  ars>P_10 = K_1 * ars>P_00;


209  ars>P_11 = K_1 * ars>P_01;


210  }

