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

Committer:
cdonate
Date:
2012-08-15
Revision:
0:8a460b0d6f09

File content as of revision 0:8a460b0d6f09:

#ifndef KALMAN_H 
#define KALMAN_H

#define R_matrix          0.590 
#define Q_Gyro_matrix     0.002 
#define Q_Accel_matrix    0.001
 
typedef struct 
{ 
    // Two states, angle and gyro bias. Unbiased angular rate is a byproduct. 
    double x_bias;
    double x_rate; 
    double x_angle; 
 
    // Covariance of estimation error matrix. 
    double P_00; 
    double P_01; 
    double P_10; 
    double P_11; 
 
    // State constants. 
    //double dt; 
    double R_angle; 
    double Q_gyro; 
    double Q_angle; 
     
} kalman; 
 
 
void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle); 
void kalman_predict(kalman *filter, double dot_angle,  double dt); 
void kalman_update(kalman *filter, double angle_measured); 
 
// Get the bias. 
double kalman_get_bias(kalman *filter) 
{ 
    return filter->x_bias; 
} 
// Get the rate. 
double kalman_get_rate(kalman *filter) 
{ 
    return filter->x_rate; 
} 
// Get the angle. 
double kalman_get_angle(kalman *filter) 
{ 
    return filter->x_angle; 
} 
 
#endif