卡尔曼滤波中关键参数的调整 来自知乎上官致远

15519743871

卡尔曼滤波中关键参数的调整
上官致远

机器人、运动控制、电机驱动技术







写在前面

对于卡尔曼滤波，大多数人仅限于会用，很少有人能透彻的理解，而关于卡尔曼滤波中那几个关键参数的调整更是一头雾水。自从写完「深度解析卡尔曼滤波在IMU中的使用」一文，很多朋友咨询卡尔曼滤波的参数到底怎么调整，工作之余总结一下，由于水平有限，错误之处在所难免，欢迎交流指正。

本篇还是以飞控与移动机器人中常用的IMU芯片MPU6050为例，详细讲解卡尔曼滤波中那几个关键参数的调整。

源码参考 TKJElectronics/KalmanFilter
Kalman::Kalman() {    /* We will set the variables like so, these can also be tuned by the user */    Q_angle = 0.001f;    Q_bias = 0.003f;    R_measure = 0.03f;    angle = 0.0f; // Reset the angle    bias = 0.0f; // Reset bias    P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical    P[0][1] = 0.0f;    P[1][0] = 0.0f;    P[1][1] = 0.0f;};// The angle should be in degrees and the rate should be in degrees per second and the delta time in secondsfloat Kalman::getAngle(float newAngle, float newRate, float dt) {    // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145    // Modified by Kristian Lauszus    // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it    // Discrete Kalman filter time update equations - Time Update ("Predict")    // Update xhat - Project the state ahead    /* Step 1 */    rate = newRate - bias;    angle += dt * rate;    // Update estimation error covariance - Project the error covariance ahead    /* Step 2 */    P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);    P[0][1] -= dt * P[1][1];    P[1][0] -= dt * P[1][1];    P[1][1] += Q_bias * dt;    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")    // Calculate Kalman gain - Compute the Kalman gain    /* Step 4 */    float S = P[0][0] + R_measure; // Estimate error    /* Step 5 */    float K[2]; // Kalman gain - This is a 2x1 vector    K[0] = P[0][0] / S;    K[1] = P[1][0] / S;    // Calculate angle and bias - Update estimate with measurement zk (newAngle)    /* Step 3 */    float y = newAngle - angle; // Angle difference    /* Step 6 */    angle += K[0] * y;    bias += K[1] * y;    // Calculate estimation error covariance - Update the error covariance    /* Step 7 */    float P00_temp = P[0][0];    float P01_temp = P[0][1];    P[0][0] -= K[0] * P00_temp;    P[0][1] -= K[0] * P01_temp;    P[1][0] -= K[1] * P00_temp;    P[1][1] -= K[1] * P01_temp;    return angle;};void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting anglefloat Kalman::getRate() { return this->rate; }; // Return the unbiased rate/* These are used to tune the Kalman filter */void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };float Kalman::getQangle() { return this->Q_angle; };float Kalman::getQbias() { return this->Q_bias; };float Kalman::getRmeasure() { return this->R_measure; };
关键参数

使用卡尔曼滤波时需要读取系统起始角度，就是通过读取加速度计的值，然后根据反正切计算出的系统倾角。

卡尔曼滤波中的几个关键参数包括dt、Q_angle、Q_bias、R_measure、P。
前缀Q表示预测（过程）噪声方差;
前缀R表示测量（观测）噪声方差;
dt表示采样时间,它的值要特别精确；
Q_angle表示加速度计过程噪声协方差;
Q_bias表示陀螺仪过程噪声协方差;
R_measure表示测量噪声协方差;
P表示误差协方差矩阵;

下面简单的看一下当改变参数时系统的输出变化。直接通过读取加速度计的值计算出的角度，我们称为accRaw图中红线表示;通过陀螺仪积分计算的角度，我们称为gyroDt图中白线表示，黄线和白线贴的比较紧，需要放大才能看出;卡尔曼滤波计算的角度，我们称为kalman图中黄线表示;

当dt过小时，黄线虽然能跟随红线，但是非常滞后:
dt=0.001

当dt过大时，黄线能跟随红线但明显过冲:
dt=0.010

dt=0.005时黄线可以很好的跟随红线:

当Q_angle越大时，黄线跟随红线趋势越紧密，滤波之后的噪声也越大，下图的噪声明显增大

Q_angle=1.0

缩小看一下

Q_angle=0.01黄线跟随红线趋势变缓，滤波效果改善明显

Q_bias越小白线跟随黄线越紧密
Q_bias=0.3

Q_bias=0.003

R_measure值的改变主要是影响卡尔曼的收敛速度，由于测试设备简陋，测试时并没有得到较好的波形输出效果的比较。

参数解析

Q值为过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；但是太小则容易发散，如果Q为零，那么我们只相信预测值；Q值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；如果Q值无穷大，那么我们只信任测量值；R值为测量噪声，太小太大都不一定合适。R太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；越小系统收敛越快，但过小则容易出现震荡；测试时可以保持陀螺仪不动，记录一段时间内陀螺仪的输出数据，这个数据近似正态分布，按3σ原则，取正态分布的(3σ)^2作为R的初始化值。

测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。

系统中还有一个关键值P，它是误差协方差初始值，表示我们对当前预测状态的信任度，它越小说明我们越相信当前预测状态；它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。随着卡尔曼滤波的迭代，P的值会不断的改变，当系统进入稳态之后P值会收敛成一个最小的估计方差矩阵，这个时候的卡尔曼增益也是最优的，所以这个值只是影响初始收敛速度。

结语

在实际工程中大家可能都用过PID，算法很简单，但是要想调好PID，实现一定的精度和速度却不是那么容易。这个时候就需要你对系统有很深的理解。卡尔曼滤波的核心代码也就那么几行，但参数的整定却不是那么容易。从反馈角度看，卡尔曼滤波也是一个线性时变反馈系统，逐渐收敛于一个线性时不变系统。最大似然估计方面、反馈系统方面、残差方面以及动态特性方面等都是分析卡尔曼滤波参数的关键之处。在很多工业控制领域，比如飞船的控制、卫星的控制等都有专门的负责调参的工程师甚至是一个团队，可见卡尔曼滤波参数整定的重要程度。从某种意义上说，调参更像是一门艺术。

针对于同一套工程模型，这些参数理论上应该可以统一，但是不同环境下系统的过程噪声以及测量噪声有可能不同，因此还是需要实际测试，以便于找到参数的最优值。

参考:

xiahouzuoxin/kalman_filter
TKJElectronics/KalmanFilter
[Fundamentals of Kalman Filtering A Practical Approach]
Kalman filter