一、一维卡尔曼滤波公式
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值会收敛成一个最小的估计方差矩阵,这个时候的卡尔曼增益也是最优的,所以这个值只是影响初始收敛速度。
三、参考
《How a Kalman filter works, in pictures》