卡尔曼滤波.rar_卡尔曼_卡尔曼滤波

/** ***************************************************************************************** * @file kalman.c * * @author Liu heng * * @version V1.0.0 * * @date 27-August-2013 * * @brief 一维卡尔曼滤波器的具体实现。实现过程完全与硬件无关， * * 可直接调用，任意移植。 * * 使用时先定义一个kalman指针，然后调用kalmanCreate()创建一个滤波器�** * 每次读取到传感器数据后即可调用KalmanFilter()来对数据进行滤波。 * ***************************************************************************************** * 使用示例 * * kalman p; * * float SersorData; * * kalmanCreate(&p,20,200); * * while(1) * * { * * SersorData = sersor(); * * SersorData = KalmanFilter(&p,SersorData); * * printf("%2.2f",SersorData); * * } * ***************************************************************************************** * MPU6050的卡尔曼滤波器参考参数 Q：10 R：400 * *****************************************************************************************/ #include "kalman.h" /** * @name kalmanCreate * @brief 创建一个卡尔曼滤波器 * @param p: 滤波器 * T_Q:系统噪声协方差 * T_R:测量噪声协方差 * * @retval none */ void kalmanCreate(kalman *p,float T_Q,float T_R) { //kalman* p = ( kalman*)malloc(sizeof( kalman)); p->X_last = (float)0; p->P_last = 0; p->Q = T_Q; p->R = T_R; p->A = 1; p->H = 1; p->X_mid = p->X_last; //return p; } /** * @name KalmanFilter * @brief 卡尔曼滤波器 * @param p: 滤波器 * dat:待滤波数据 * @retval 滤波后的数据 */ float KalmanFilter(kalman* p,float dat) { p->X_mid =p->A*p->X_last; //x(k|k-1) = AX(k-1|k-1)+BU(k) p->P_mid = p->A*p->P_last+p->Q; //p(k|k-1) = Ap(k-1|k-1)A'+Q p->kg = p->P_mid/(p->P_mid+p->R); //kg(k) = p(k|k-1)H'/(Hp(k|k-1)'+R) p->X_now = p->X_mid+p->kg*(dat-p->X_mid); //x(k|k) = X(k|k-1)+kg(k)(Z(k)-HX(k|k-1)) p->P_now = (1-p->kg)*p->P_mid; //p(k|k) = (I-kg(k)H)P(k|k-1) p->P_last = p->P_now; //状态更新 p->X_last = p->X_now; return p->X_now; }