Diff: kalman.h

Revision:

5:c3caf8b83e6b

Child:

8:dc48ce79ad59

diff -r a5b51a651db7 -r c3caf8b83e6b kalman.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h	Fri Dec 19 13:33:21 2014 +0000
@@ -0,0 +1,41 @@
+/*
+ * kalman.h
+ *
+ *  Created on: Aug 21, 2014
+ *      Author: Michal
+ */
+
+#ifndef KALMAN_H_
+#define KALMAN_H_
+
+//#define DT 0.00225f // 100Hz
+#define DT 0.005   //kalman2 z wyswietlaniem
+//#define DT 0.002471 //kalman2 bez wyswietlania
+//#define DT 0.0055  //kalman wyswietlanie pitch katx
+//#define DT 0.00252 //klaman2 + filtr dolno
+//#define DT 0.002472 //klaman2 + filtr dolno x2
+//#define DT 0.00223 //klaman2 + filtr dolno x2 bez kompl
+//  Q matrix elements
+#define Q1 0.2f            //5  //10
+#define Q2 2.0f      //80       //120
+#define Q3 0.01f
+
+// R matrix elements
+#define R1 900.0f       //90      //tu duzo   //10000  //500
+#define R2 1500.0f      //150      //tu mniej  //12000  //600
+
+struct _kalman_data
+{
+    float x1, x2, x3;
+    float p11, p12, p13, p21, p22, p23, p31, p32, p33;
+    float q1,q2,q3,r1,r2;
+};
+
+typedef struct _kalman_data kalman_data;
+
+void kalman_innovate(kalman_data *data, float z1, float z2);
+void kalman_init(kalman_data *data);
+
+
+
+#endif /* KALMAN_H_ */