source: flair-src/trunk/lib/FlairFilter/src/AhrsKalman_impl.cpp@ 137

Last change on this file since 137 was 18, checked in by Sanahuja Guillaume, 6 years ago

ahrs kalman

File size: 7.2 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C 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
62using std::string;
63using namespace flair::core;
64using namespace flair::gui;
65
66AhrsKalman_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
82AhrsKalman_impl::~AhrsKalman_impl() {
83}
84
85void 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 rawGyr-Vector3D(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
126void 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 model-state 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 */
163void 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 */
196void 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}
Note: See TracBrowser for help on using the repository browser.