-
Notifications
You must be signed in to change notification settings - Fork 0
/
kalman.h
50 lines (40 loc) · 1.04 KB
/
kalman.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
#ifndef _KALMAN_H_
#define _KALMAN_H_
#include "stm32f4xx.h"
typedef void (*timeUpdateFunc)(float*,void*,void*);
typedef void (*EstiMeasureFunc)(float*,void*,void*);
typedef void (*getA_function)(float*,void*,void*,void*);
typedef void (*getH_function)(float*,void*,void*);
typedef struct
{
u8 state_dim;
u8 measure_dim;
float *x;
float *y;
float *P;
float *A;
float *H;
float *Q;
float *R;
getA_function GetA;
getH_function GetH;
timeUpdateFunc aFunc;
EstiMeasureFunc hFunc;
}ekf_filter_t;
typedef ekf_filter_t* ekf_filter;
ekf_filter ekf_filter_new(u8 state_dim,
u8 measure_dim,
const float *Q,
const float *R,
getA_function GetA,
getH_function GetH,
timeUpdateFunc aFunc,
EstiMeasureFunc hFunc);
/*
* delete a ekf filter
*/
void ekf_filter_delete(ekf_filter filter);
void EKF_predict(ekf_filter filter,void *para1,void *para2,void *para3,void *para4,void *para5);
void EKF_update(ekf_filter filter,float *measure, void *para1,void *para2, void *para3, void *para4);
void printMat(float *mat,u8 row,u8 col);
#endif