QuadroCopter  0.1.4
Kalman.h
Go to the documentation of this file.
1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
2 
3  This software may be distributed and modified under the terms of the GNU
4  General Public License version 2 (GPL2) as published by the Free Software
5  Foundation and appearing in the file GPL2.TXT included in the packaging of
6  this file. Please note that GPL2 Section 2[b] requires that all works based
7  on this software must also be made publicly available under the terms of
8  the GPL2 ("Copyleft").
9 
10  Contact information
11  -------------------
12 
13  Kristian Lauszus, TKJ Electronics
14  Web : http://www.tkjelectronics.com
15  e-mail : kristianl@tkjelectronics.com
16  */
17 
18 #ifndef _Kalman_h
19 #define _Kalman_h
20 
21 class Kalman {
22 public:
23  Kalman() {
24  /* We will set the variables like so, these can also be tuned by the user */
25  Q_angle = 0.001;
26  Q_bias = 0.003;
27  R_measure = 0.03;
28 
29  angle = 0; // Reset the angle
30  bias = 0; // Reset bias
31 
32  P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
33  P[0][1] = 0;
34  P[1][0] = 0;
35  P[1][1] = 0;
36  };
37  // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
38  double getAngle(double newAngle, double newRate, double dt) {
39  // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
40  // Modified by Kristian Lauszus
41  // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
42 
43  // Discrete Kalman filter time update equations - Time Update ("Predict")
44  // Update xhat - Project the state ahead
45  /* Step 1 */
46  rate = newRate - bias;
47  angle += dt * rate;
48 
49  // Update estimation error covariance - Project the error covariance ahead
50  /* Step 2 */
51  P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
52  P[0][1] -= dt * P[1][1];
53  P[1][0] -= dt * P[1][1];
54  P[1][1] += Q_bias * dt;
55 
56  // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
57  // Calculate Kalman gain - Compute the Kalman gain
58  /* Step 4 */
59  S = P[0][0] + R_measure;
60  /* Step 5 */
61  K[0] = P[0][0] / S;
62  K[1] = P[1][0] / S;
63 
64  // Calculate angle and bias - Update estimate with measurement zk (newAngle)
65  /* Step 3 */
66  y = newAngle - angle;
67  /* Step 6 */
68  angle += K[0] * y;
69  bias += K[1] * y;
70 
71  // Calculate estimation error covariance - Update the error covariance
72  /* Step 7 */
73  P[0][0] -= K[0] * P[0][0];
74  P[0][1] -= K[0] * P[0][1];
75  P[1][0] -= K[1] * P[0][0];
76  P[1][1] -= K[1] * P[0][1];
77 
78  return angle;
79  };
80  void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
81  double getRate() { return rate; }; // Return the unbiased rate
82 
83  /* These are used to tune the Kalman filter */
84  void setQangle(double newQ_angle) { Q_angle = newQ_angle; };
85  void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
86  void setRmeasure(double newR_measure) { R_measure = newR_measure; };
87 
88  double getQangle() { return Q_angle; };
89  double getQbias() { return Q_bias; };
90  double getRmeasure() { return R_measure; };
91 
92 private:
93  /* Kalman filter variables */
94  double Q_angle; // Process noise variance for the accelerometer
95  double Q_bias; // Process noise variance for the gyro bias
96  double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
97 
98  double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
99  double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
100  double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
101 
102  double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
103  double K[2]; // Kalman gain - This is a 2x1 vector
104  double y; // Angle difference
105  double S; // Estimate error
106 };
107 
108 #endif
double getRmeasure()
Definition: Kalman.h:90
void setRmeasure(double newR_measure)
Definition: Kalman.h:86
double getQangle()
Definition: Kalman.h:88
double getAngle(double newAngle, double newRate, double dt)
Definition: Kalman.h:38
void setAngle(double newAngle)
Definition: Kalman.h:80
double getRate()
Definition: Kalman.h:81
void setQangle(double newQ_angle)
Definition: Kalman.h:84
void setQbias(double newQ_bias)
Definition: Kalman.h:85
double getQbias()
Definition: Kalman.h:89
Definition: Kalman.h:21
Kalman()
Definition: Kalman.h:23