/**
******************************************************************************
* @brief 卡尔曼滤波器 函数
* @param inData - 输入值
* @return 滤波后的值
* @note r值固定,q值越大,代表越信任测量值,q值无穷大,代表只用测量值。
* q值越小,代表越信任模型预测值,q值为0,代表只用模型预测值。
* q:过程噪声,q增大,动态响应变快,收敛稳定性变坏;反之。控制误差
* r:测量噪声,r增大,动态响应变慢,收敛稳定性变好;反之。控制响应速度
******************************************************************************
*/
unsigned long KalmanFilter(unsigned long inData)
{
static float xdata kalman = 0; //上次卡尔曼值(估计出的最优值)
static float xdata p = 10;
float xdata q = 0.001; //q:过程噪声
float xdata r = 0.001; //r:测量噪声
float xdata kg = 0; //kg:卡尔曼增益
p += q;
kg = p / ( p + r ); //计算卡尔曼增益
kalman = kalman + (kg * (inData - kalman)); //计算本次滤波估计值
p = (1 - kg) * p; //更新测量方差
return (unsigned long)kalman; //返回估计值
}
卡尔曼滤波算法 C语言实现
最新推荐文章于 2024-03-04 19:22:05 发布