Claudio Donate / kalman

Library for a Kalman filter that receive inputs from accelerometer and gyroscope and outputs PITCH and ROLL

Dependents:   KalmanFilter Freescale_Multi-Sensor_Quadcopter FRDM_EMG_Galileo_Hand Galileo_HoverBoardRider ... more

kalman.h@0:8a460b0d6f09, 2012-08-15 (annotated)

Committer:
cdonate
Date:
Wed Aug 15 22:17:07 2012 +0000
Revision:
0:8a460b0d6f09
Kalman Filter fusing input from the Accelerometer and Gyroscope to output PITCH and ROLL.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cdonate 0:8a460b0d6f09 1 #ifndef KALMAN_H
cdonate 0:8a460b0d6f09 2 #define KALMAN_H
cdonate 0:8a460b0d6f09 3
cdonate 0:8a460b0d6f09 4 #define R_matrix 0.590
cdonate 0:8a460b0d6f09 5 #define Q_Gyro_matrix 0.002
cdonate 0:8a460b0d6f09 6 #define Q_Accel_matrix 0.001
cdonate 0:8a460b0d6f09 7
cdonate 0:8a460b0d6f09 8 typedef struct
cdonate 0:8a460b0d6f09 9 {
cdonate 0:8a460b0d6f09 10 // Two states, angle and gyro bias. Unbiased angular rate is a byproduct.
cdonate 0:8a460b0d6f09 11 double x_bias;
cdonate 0:8a460b0d6f09 12 double x_rate;
cdonate 0:8a460b0d6f09 13 double x_angle;
cdonate 0:8a460b0d6f09 14
cdonate 0:8a460b0d6f09 15 // Covariance of estimation error matrix.
cdonate 0:8a460b0d6f09 16 double P_00;
cdonate 0:8a460b0d6f09 17 double P_01;
cdonate 0:8a460b0d6f09 18 double P_10;
cdonate 0:8a460b0d6f09 19 double P_11;
cdonate 0:8a460b0d6f09 20
cdonate 0:8a460b0d6f09 21 // State constants.
cdonate 0:8a460b0d6f09 22 //double dt;
cdonate 0:8a460b0d6f09 23 double R_angle;
cdonate 0:8a460b0d6f09 24 double Q_gyro;
cdonate 0:8a460b0d6f09 25 double Q_angle;
cdonate 0:8a460b0d6f09 26
cdonate 0:8a460b0d6f09 27 } kalman;
cdonate 0:8a460b0d6f09 28
cdonate 0:8a460b0d6f09 29
cdonate 0:8a460b0d6f09 30 void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle);
cdonate 0:8a460b0d6f09 31 void kalman_predict(kalman *filter, double dot_angle, double dt);
cdonate 0:8a460b0d6f09 32 void kalman_update(kalman *filter, double angle_measured);
cdonate 0:8a460b0d6f09 33
cdonate 0:8a460b0d6f09 34 // Get the bias.
cdonate 0:8a460b0d6f09 35 double kalman_get_bias(kalman *filter)
cdonate 0:8a460b0d6f09 36 {
cdonate 0:8a460b0d6f09 37 return filter->x_bias;
cdonate 0:8a460b0d6f09 38 }
cdonate 0:8a460b0d6f09 39 // Get the rate.
cdonate 0:8a460b0d6f09 40 double kalman_get_rate(kalman *filter)
cdonate 0:8a460b0d6f09 41 {
cdonate 0:8a460b0d6f09 42 return filter->x_rate;
cdonate 0:8a460b0d6f09 43 }
cdonate 0:8a460b0d6f09 44 // Get the angle.
cdonate 0:8a460b0d6f09 45 double kalman_get_angle(kalman *filter)
cdonate 0:8a460b0d6f09 46 {
cdonate 0:8a460b0d6f09 47 return filter->x_angle;
cdonate 0:8a460b0d6f09 48 }
cdonate 0:8a460b0d6f09 49
cdonate 0:8a460b0d6f09 50 #endif