File content as of revision 8:dc48ce79ad59:

/*
 * 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 500.0f  //900.0f       //90      //tu duzo   //10000  //500
#define R2 600.0f  //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_ */