我加入了个浮点输入计算的函数，我理论计算出来应该是小数，但是用程序运行之后，结果都是0；我试着将结果放大之后也是一样的。代码如下：
Code: Select all
uint8_t Kalman_Filter_Algorithm(float calculation_vaule, float Q, float R, float measure_vaule)
{
static float x_last = 0;
static float p_last = 0;
static float kg;
static float x_mid;
static float x_now;
static float p_mid;
static float p_now;
/*
Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
R:测量噪声，R增大，动态响应变慢，收敛稳定性变好
*/
if ((measure_vaule > calculation_vaule) && ((measure_vaule - calculation_vaule) > 10))
{
measure_vaule = calculation_vaule + 10;
}
else if ((calculation_vaule > measure_vaule) && ((calculation_vaule - measure_vaule) > 10))
{
measure_vaule = calculation_vaule - 10;
}
else
{
;
}
x_now = calculation_vaule;
// 先验估算值
x_mid = calculation_vaule;
// 先验协方差
p_mid = p_last + Q;
// 卡尔曼增益
kg = p_mid / (p_mid + R);
// 最优估计值
x_now = x_mid + kg*(measure_vaule - x_mid);
// 最新协方差
p_now = (1 - kg)*p_mid;
p_last = p_now;
x_last = x_now;
return (uint8_t)x_now;
}