Diff: kalman.h

Revision:

0:8a460b0d6f09

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h	Wed Aug 15 22:17:07 2012 +0000
@@ -0,0 +1,50 @@
+#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
\ No newline at end of file