一、一维卡尔曼滤波公式
double KalmanFilterLossPr(double x) const {
#define KALMAN_Q 实测调整
#define KALMAN_R 实测调整
double R = KALMAN_R;
double Q = KALMAN_Q;
static double x_last;//任意配置
double x_mid = x_last;
double x_now;
static double p_last;//不能为0
double p_mid ;
double p_now;
double kg;
x_mid = x_last;
p_mid = p_last;
kg = p_mid / (p_mid + R);
x_now = x_mid + kg * (x - x_mid);
p_now = (1 - kg) * p_mid + Q;
p_last = p_now;
x_last = x_now;
return x_now;
}二、参数说明：
Q：过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；但是太小则容易发散，如果Q为零，那么我们只相信预测值；Q值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；如果Q值无穷大，那么我们只信任测量值；
R：测量噪声，太小太大都不一定合适。R太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；越小系统收敛越快，但过小则容易出现震荡；
P：误差协方差初始值，表示我们对当前预测状态的信任度。它越小说明我们越相信当前预测状态；它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。随着卡尔曼滤波的迭代，P的值会不断的改变，当系统进入稳态之后P值会收敛成一个最小的估计方差矩阵，这个时候的卡尔曼增益也是最优的，所以这个值只是影响初始收敛速度。
三、参考
