10 lines
269 B
C
10 lines
269 B
C
#include "kalman.h"
|
|
|
|
//一维卡尔曼滤波
|
|
void kalmanfiter(struct KalmanFilter *EKF,float input)
|
|
{
|
|
EKF->NewP = EKF->LastP + EKF->Q;
|
|
EKF->Kg = EKF->NewP / (EKF->NewP + EKF->R);
|
|
EKF->Out = EKF->Out + EKF->Kg * (input - EKF->Out);
|
|
EKF->LastP = (1 - EKF->Kg) * EKF->NewP;
|
|
} |