目录
1、 为什么使用卡尔曼滤波
1、滤波效果好
2、实时采样要求高
3、采样速度高
4、算法实现简单
经过滤波后曲线比较平滑
2、 C++算法
#ifndef KALMANFILTER_H
#define KALMANFILTER_H
///卡尔曼滤波器结构体类型定义
typedef struct
{
float x_last; // 上次的数据
float P_last; //
float Q; // 计算偏差,该值越大越相信输入值z_real
float R; // 测量偏差,该值越大越相信上一次的数据
}KalmanFilterTypeDef;//卡尔曼滤波器
class Kalman
{
public:
float KalmanFilter(KalmanFilterTypeDef* Filter, float z_real)
{
float P_temp = Filter->P_last + Filter->Q;
float K = P_temp/(P_temp + Filter->R);
Filter->x_last = Filter->x_last +K*(z_real - Filter->x_last);
Filter->P_last = (1 - K) * P_temp;
return Filter->x_last;
}
/* Q计算偏差 越大越相信真实值
* R测量偏差 越大越相信预测值
*/
void KalmanFilterInit(float Q, float R, KalmanFilterTypeDef* Filter)
{
Filter->Q = Q;
Filter->R = R;
Filter->P_last = 0;
Filter->x_last = 0;
return;
}
};
#endif // KALMANFILTER_H