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?
User | Revision | Line number | New 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 |