10 lines
269 B
C
10 lines
269 B
C
|
#include "kalman.h"
|
|||
|
|
|
|||
|
|
//һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>
|
|||
|
|
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;
|
|||
|
|
}
|