David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

Kalman.h@34:eaea0ae92dfa, 2016-04-24 (annotated)

Committer:
ivo_david_michelle
Date:
Sun Apr 24 17:15:20 2016 +0000
Revision:
34:eaea0ae92dfa
testing with kalman

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 34:eaea0ae92dfa 1 #ifndef _Kalman_h_
ivo_david_michelle 34:eaea0ae92dfa 2 #define _Kalman_h_
ivo_david_michelle 34:eaea0ae92dfa 3
ivo_david_michelle 34:eaea0ae92dfa 4 class Kalman {
ivo_david_michelle 34:eaea0ae92dfa 5 public:
ivo_david_michelle 34:eaea0ae92dfa 6 Kalman();
ivo_david_michelle 34:eaea0ae92dfa 7
ivo_david_michelle 34:eaea0ae92dfa 8 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
ivo_david_michelle 34:eaea0ae92dfa 9 float getAngle(float newAngle, float newRate, float dt);
ivo_david_michelle 34:eaea0ae92dfa 10
ivo_david_michelle 34:eaea0ae92dfa 11 void setAngle(float angle); // Used to set angle, this should be set as the starting angle
ivo_david_michelle 34:eaea0ae92dfa 12 float getRate(); // Return the unbiased rate
ivo_david_michelle 34:eaea0ae92dfa 13
ivo_david_michelle 34:eaea0ae92dfa 14 /* These are used to tune the Kalman filter */
ivo_david_michelle 34:eaea0ae92dfa 15 void setQangle(float Q_angle);
ivo_david_michelle 34:eaea0ae92dfa 16 void setQbias(float Q_bias);
ivo_david_michelle 34:eaea0ae92dfa 17 void setRmeasure(float R_measure);
ivo_david_michelle 34:eaea0ae92dfa 18
ivo_david_michelle 34:eaea0ae92dfa 19 float getQangle();
ivo_david_michelle 34:eaea0ae92dfa 20 float getQbias();
ivo_david_michelle 34:eaea0ae92dfa 21 float getRmeasure();
ivo_david_michelle 34:eaea0ae92dfa 22
ivo_david_michelle 34:eaea0ae92dfa 23 private:
ivo_david_michelle 34:eaea0ae92dfa 24 /* Kalman filter variables */
ivo_david_michelle 34:eaea0ae92dfa 25 float Q_angle; // Process noise variance for the accelerometer
ivo_david_michelle 34:eaea0ae92dfa 26 float Q_bias; // Process noise variance for the gyro bias
ivo_david_michelle 34:eaea0ae92dfa 27 float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
ivo_david_michelle 34:eaea0ae92dfa 28
ivo_david_michelle 34:eaea0ae92dfa 29 float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
ivo_david_michelle 34:eaea0ae92dfa 30 float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
ivo_david_michelle 34:eaea0ae92dfa 31 float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
ivo_david_michelle 34:eaea0ae92dfa 32
ivo_david_michelle 34:eaea0ae92dfa 33 float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
ivo_david_michelle 34:eaea0ae92dfa 34 };
ivo_david_michelle 34:eaea0ae92dfa 35
ivo_david_michelle 34:eaea0ae92dfa 36 #endif