YUFEI JIANG
CSSE4011_BLE_IMU IMU Seeed Tiny Ble
Dependencies: BLE_API_Tiny_BLE MPU6050-DMP-Seeed-Tiny-BLE mbed
Diff: kalman.h
- Revision:
- 0:f90c3452d779
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h Wed Jun 10 09:46:16 2015 +0000
@@ -0,0 +1,36 @@
+#ifndef __KALMAN_H__
+#define __KALMAN_H__
+
+#define DT 0.01f // 100Hz
+
+// Q diagonal 3x3 with these elements on diagonal
+#define Q1 5.0f
+#define Q2 100.0f
+#define Q3 0.01f
+
+// R diagonal 2x2 with these elements on diagonal
+#define R1 1000.0f
+#define R2 1000.0f
+
+
+
+
+struct _kalman_data
+{
+ float x1, x2, x3;
+ float p11, p12, p13, p21, p22, p23, p31, p32, p33;
+ float q1, q2, q3;
+ float r1, r2;
+};
+
+typedef struct _kalman_data kalman_data;
+
+void kalman_innovate(kalman_data *data, float z1, float z2);
+//void kalman_init(void);
+void kalman_init(kalman_data *data);
+
+#endif
+
+
+
+