代码语言：javascript复制
int KalmanFilter(uint16_t data)
{
//上一个ADC的值
static uint16_t prevdata = 0;
//q表示误差，r表示响应速度，参数根据自身需求进行调整
static float p = 10 , q = 0.001,r = 0.001,kGain = 0;
p = p q;
kGain = p / ( p r);
data = prevdata (kGain * (data - prevdata ));
p = (1 - kGain) * p;
prevdata = data;
return data;
} 在卡尔曼滤波器中，p 代表估计误差的协方差，它反映了我们对当前估计值的不确定度。p 的初始值设定依赖于你对系统初始状态不确定性的了解。初始值 p = 10 是根据经验或特定应用场景设定的一个值，它表示开始时对估计值的不确定性程度。在这个简化版的卡尔曼滤波器中，q 代表了预测噪声的协方差，它衡量了预测步骤中引入的不确定性；而 r 代表了观测噪声的协方差，它衡量了观测值本身的不确定性。
假设我现在输入以下几个数值进行比较，4094, 4092, 4093（4095为满ADC值）
1.对第一个值4094进行滤波处理
初始时，prevdata = 0，p = 10，q = 0.001，r = 0.001
- 计算
kGain：kGain = p / (p r) = 10 / (10 0.001) ≈ 0.999 - 更新估计值：
data = prevdata (kGain * (4094 - prevdata)) = 0 (0.999 * 4094) ≈ 4093 - 更新
p：p = (1 - kGain) * p = (1 - 0.999) * 10 ≈ 0.1 - 更新
prevdata：prevdata = data ≈ 4093
2.对第二个值4092继续滤波处理
- 计算
kGain：kGain = p / (p r) = 0.1 / (0.1 0.001) ≈ 0.990 - 更新估计值：
data = prevdata (kGain * (4092 - prevdata)) ≈ 4093 (0.990 * (4092 - 4093)) ≈ 4091 - 更新
p：p ≈ 0.01 - 更新
prevdata：prevdata ≈ 4091
3.对第三个值4093滤波处理
- 计算
kGain：kGain ≈ p /(p r) - 更新估计值：
data ≈ prevdata (kGain * (4093 - prevdata)) - 更新
p：p的值会进一步减小 - 更新
prevdata：prevdata将被更新为新的估计值
每次迭代都会更新 p，使得滤波器对新的观测值更加信任，并逐渐减小对初始估计值的不确定度。因此，随着迭代次数的增加，kGain 会逐渐减小，滤波器对新观测值的反应也会逐渐变得平滑。
