Mechatronics / 2015_04_17_dydakt

Dydaktyka

Dependencies:   FastPWM mbed-src

Fork of 2015_04_17_quadro_bez_sterowania by Quadrocopter

kalman.h@9:4e94a16ca90c, 2015-01-29 (annotated)

Committer:
Michu90
Date:
Thu Jan 29 14:17:43 2015 +0000
Revision:
9:4e94a16ca90c
Parent:
8:dc48ce79ad59
Child:
12:e955cc086c42
pomiar omegi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Michu90 5:c3caf8b83e6b 1 /*
Michu90 5:c3caf8b83e6b 2 * kalman.h
Michu90 5:c3caf8b83e6b 3 *
Michu90 5:c3caf8b83e6b 4 * Created on: Aug 21, 2014
Michu90 5:c3caf8b83e6b 5 * Author: Michal
Michu90 5:c3caf8b83e6b 6 */
Michu90 5:c3caf8b83e6b 7
Michu90 5:c3caf8b83e6b 8 #ifndef KALMAN_H_
Michu90 5:c3caf8b83e6b 9 #define KALMAN_H_
Michu90 5:c3caf8b83e6b 10
Michu90 5:c3caf8b83e6b 11 //#define DT 0.00225f // 100Hz
Michu90 5:c3caf8b83e6b 12 #define DT 0.005 //kalman2 z wyswietlaniem
Michu90 5:c3caf8b83e6b 13 //#define DT 0.002471 //kalman2 bez wyswietlania
Michu90 5:c3caf8b83e6b 14 //#define DT 0.0055 //kalman wyswietlanie pitch katx
Michu90 5:c3caf8b83e6b 15 //#define DT 0.00252 //klaman2 + filtr dolno
Michu90 5:c3caf8b83e6b 16 //#define DT 0.002472 //klaman2 + filtr dolno x2
Michu90 5:c3caf8b83e6b 17 //#define DT 0.00223 //klaman2 + filtr dolno x2 bez kompl
Michu90 5:c3caf8b83e6b 18 // Q matrix elements
Michu90 9:4e94a16ca90c 19 #define Q1 0.2f //0.2f //5 //10
Michu90 9:4e94a16ca90c 20 #define Q2 2.0f //2.0f //80 //120
Michu90 9:4e94a16ca90c 21 #define Q3 0.01 //0.01f
Michu90 5:c3caf8b83e6b 22
Michu90 5:c3caf8b83e6b 23 // R matrix elements
Michu90 9:4e94a16ca90c 24 #define R1 120 //500.0f //900.0f //90 //tu duzo //10000 //500
Michu90 9:4e94a16ca90c 25 #define R2 150 //600.0f //1500.0f //150 //tu mniej //12000 //600
Michu90 5:c3caf8b83e6b 26
Michu90 5:c3caf8b83e6b 27 struct _kalman_data
Michu90 5:c3caf8b83e6b 28 {
Michu90 5:c3caf8b83e6b 29 float x1, x2, x3;
Michu90 5:c3caf8b83e6b 30 float p11, p12, p13, p21, p22, p23, p31, p32, p33;
Michu90 5:c3caf8b83e6b 31 float q1,q2,q3,r1,r2;
Michu90 5:c3caf8b83e6b 32 };
Michu90 5:c3caf8b83e6b 33
Michu90 5:c3caf8b83e6b 34 typedef struct _kalman_data kalman_data;
Michu90 5:c3caf8b83e6b 35
Michu90 5:c3caf8b83e6b 36 void kalman_innovate(kalman_data *data, float z1, float z2);
Michu90 5:c3caf8b83e6b 37 void kalman_init(kalman_data *data);
Michu90 5:c3caf8b83e6b 38
Michu90 5:c3caf8b83e6b 39
Michu90 5:c3caf8b83e6b 40
Michu90 5:c3caf8b83e6b 41 #endif /* KALMAN_H_ */