卡尔曼滤波中关键参数的调整

写在前面

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


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

源码参考 TKJElectronics/KalmanFilter

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
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 seconds
float 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 angle
float 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σ)^2作为R的初始化值。

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

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

结语

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

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

参考:

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

您的支持是我原创的动力