29 lines
670 B
C
29 lines
670 B
C
|
/**
|
|||
|
|
* @author Liu heng
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>RoboMaster<EFBFBD><EFBFBD>̳
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#ifndef _KALMAN_H
|
|||
|
|
#define _KALMAN_H
|
|||
|
|
|
|||
|
|
|
|||
|
|
typedef struct {
|
|||
|
|
float X_last; //<2F><>һʱ<D2BB>̵<EFBFBD><CCB5><EFBFBD><EFBFBD>Ž<EFBFBD><C5BD><EFBFBD> X(k-|k-1)
|
|||
|
|
float X_mid; //<2F><>ǰʱ<C7B0>̵<EFBFBD>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD> X(k|k-1)
|
|||
|
|
float X_now; //<2F><>ǰʱ<C7B0>̵<EFBFBD><CCB5><EFBFBD><EFBFBD>Ž<EFBFBD><C5BD><EFBFBD> X(k|k)
|
|||
|
|
float P_mid; //<2F><>ǰʱ<C7B0><CAB1>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD> P(k|k-1)
|
|||
|
|
float P_now; //<2F><>ǰʱ<C7B0><CAB1><EFBFBD><EFBFBD><EFBFBD>Ž<EFBFBD><C5BD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD> P(k|k)
|
|||
|
|
float P_last; //<2F><>һʱ<D2BB><CAB1><EFBFBD><EFBFBD><EFBFBD>Ž<EFBFBD><C5BD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD> P(k-1|k-1)
|
|||
|
|
float kg; //kalman<61><6E><EFBFBD><EFBFBD>
|
|||
|
|
float A; //ϵͳ<CFB5><CDB3><EFBFBD><EFBFBD>
|
|||
|
|
float B;
|
|||
|
|
float Q;
|
|||
|
|
float R;
|
|||
|
|
float H;
|
|||
|
|
}extKalman_t;
|
|||
|
|
|
|||
|
|
void KalmanCreate(extKalman_t *p,float T_Q,float T_R);
|
|||
|
|
float KalmanFilter(extKalman_t* p,float dat);
|
|||
|
|
|
|||
|
|
#endif
|