[18]  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  Vector3D 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  rawGyrVector3D(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  }

