卡尔曼滤波器的定义,实例和代码实现
卡尔曼滤波器(Kalman filter)是一种高效的递归滤波器, 能够从一系列包含噪音的测量值中估计动态系统的状态. 因为不需要存储历史状态, 没有复杂计算, 非常适合在资源有限的嵌入式系统中使用. 常用于飞行器的导引, 导航及控制, 机械和金融中的时间序列分析, 轨迹最佳化等. 卡尔曼滤波不需要假设误差是正态分布, 但如果误差属于正态分布, 卡尔曼滤波的结果会更为准确.
卡尔曼滤波的计算分二个步骤: 预测与更新. 在预测阶段, 滤波器基于上一步的预测结果, 预测当前状态和误差; 在更新阶段, 滤波器利用当前的测量值和预测值, 计算得到新的状态值和误差.
- Original Error Estimate, calculate the Kalman Gain using Error in Estimate and Error in Data(Measurement)
预测阶段, 用预测误差和测量误差计算卡尔曼增益 - Original Estimate, calculate Current Estimate using Kalman Gain, Previous Estimate and Measured Value
更新阶段, 结合测量值, 用卡尔曼增益计算当前的状态 - Calculate the new Error in Estimate
计算新的预测误差
定义
完整的卡尔曼滤波定义是这样的
-
Predict step 预测阶段
- State prediction 预测系统状态:
\(\hat{x_{t|t-1}} = F_t \hat{x_{t-1|t-1}} + B_t u_t\) - Uncertainty prediction 预测误差:
\(P_{t|t-1} = F_t P_{t-1|t-1} F_t^T + Q_t\)
- State prediction 预测系统状态:
-
Update step 更新阶段
- Kalman gain 更新卡尔曼增益:
\(K_t = \frac{P_{t|t-1} H_t^T} {H_t P_{t|t-1} H_t^T + R_t}\) - State update 更新状态:
\(\hat{x_{t|t}} = \hat{x_{t|t-1}} + K_t (z_t - H_t \hat{x_{t|t-1}})\) - Uncertainty update 更新误差:
\(P_{t|t} = (I - K_t H_t) P_{t|t-1}\)
- Kalman gain 更新卡尔曼增益:
对以上符号的说明
- \(\hat{x}\): 预测的系统状态向量
The state vector, which represents the true state of the system that we want to estimate. - \(t\): 时间序列
The time step index, corresponding to different time periods. - \(F_t\): 状态转移矩阵
The state transition matrix, which models how the system evolves from time step \(t-1\) to \(t\) without taking into account external factors. - \(B_t\): 控制输入矩阵
The control input matrix, used to incorporate the effect of any external factors \(u_t\) (e.g., motors or steer engines inputs). - \(u_t\): 控制输入向量
The control input vector, containing the external factors impacting the system. - \(P\): 误差矩阵
The uncertainty (covariance) matrix, which quantifies our uncertainty about the estimated state. - \(Q_t\): 过程噪声协方差矩阵
The process noise covariance matrix, representing the estimation error caused by our simplified model of the system state dynamics. Q矩阵表示系统模型的过程噪声, 系统模型是一个近似值, 在系统状态的整个生命周期中, 系统模型的准确性会发生波动, Q矩阵用于表示这种不确定性, 并增加了状态上的现有噪声. 例如飞行器电机的震动给加速度的读数带来的误差. - \(H_t\): 观察值转换矩阵
The observation matrix, which models how the true system state is transformed into the observed system state. - \(K_t\): 卡尔曼增益
The Kalman gain, which determines how much we trust the new observation relative to our prediction. 卡尔曼增益是一个介于0到1之间的数, 用于表示在预测中观察值所占的比重, 卡尔曼增益越大说明噪声越低, 观察值越重要. - \(z_t\): 观察值(测量值)向量
The observation (measurement) vector, containing the recorded states. - \(R_t\): 测量噪声协方差矩阵
测量噪声指的是测量工具(如传感器)测量时的固有噪声, 例如在静止时加速度传感器读数的上下波动, The observation noise covariance matrix, representing the measurement noise in the observed states. - \(I\): 单位矩阵
The identity matrix.
简化
对于一个静止(或匀速运动)的物体观测加速度和角速度, 可以忽略控制输入 \(B_t\) 和 \(u_t\), 将 \(F_t\), \(H_t\)视为单位矩阵, 卡尔曼计算公式可以简化为
-
Predict step 预测阶段,
- State prediction 预测状态等于前一步的状态:
\(\hat{x_{t|t-1}} = \hat{x_{t-1|t-1}}\) - Uncertainty prediction 预测误差等于更新后的误差加上过程噪声:
\(P_{t|t-1} = P_{t-1|t-1} + Q_t\)
- State prediction 预测状态等于前一步的状态:
-
Update step 更新阶段,
- Kalman gain 更新卡尔曼增益:
\(K_t = \frac{P_{t|t-1}} {P_{t|t-1} + R_t}\) - State update 更新状态:
\(\hat{x_{t|t}} = \hat{x_{t|t-1}} + K_t (z_t - \hat{x_{t|t-1}})\) - Uncertainty update 更新误差:
\(P_{t|t} = (I - K_t) P_{t|t-1}\)
- Kalman gain 更新卡尔曼增益:
实例
1. 初始化
令预测误差初始值为 \(P = 10000\)
测量误差\(σ = 0.1\)，方差 \(σ^2 = 0.01\), 即 \(R\) 为固定的 0.01
噪声方差为 \(q = 0.15\)
令初始预测值 \(\hat{x} = 10\)
预测误差 \(P = P + q = 10000 + 0.15 = 10000.15\)
2. 观察值 \(Z = 50.486\)
卡尔曼增益
\(K = \frac{P}{P + r} = \frac{10000.15}{10000.15 + 0.01} = 0.99999\)
更新系统状态(等于预测状态)
\(\hat{x} = \hat{x} + K * (Z - \hat{x}) = 10 + 0.99999 * (50.486 - 10) = 50.486\)
更新预测误差
\(P = (1 - K) * P = (1 - 0.99999) * 10000.15 = 0.01\)
\(P = P + q = 0.01 + 0.15 = 0.16\)
3. 观察值 \(Z = 50.963\)
卡尔曼增益
\(K = \frac{P}{P + r} = \frac{0.16}{0.16 + 0.01} = 0.9412\)
更新系统状态(等于预测状态)
\(\hat{x} = \hat{x} + K * (Z - \hat{x}) = 50.486 + 0.9412 * (50.963 - 50.486) = 50.934\)
更新预测误差
\(P = (1 - K) * P = (1 - 0.9412) * 0.16 = 0.0094\)
\(P = P + q = 0.0094 + 0.15 = 0.1594\)
可以看到 \(P\) 和 \(K\) 的值迅速收敛
实现
一个简单的C语言演示代码, 会输出每次迭代后产生的K增益, P误差和预测值.
#include <stdio.h>
const int measures[] = {
-269, -255, -130, 228, -437, -1234, 1247, 173, -400, -1561, -1038, 207, 958, -516, -581, -716, -18, -1193, -989, -593, 484, 102, 718, 1362, 1563, 2683, 428, 1616, 2922, 2968, 3046, 3572, 4006, 4821, 3964, 3127, 3086, 3190, 3682, 4015, 4471, 4211, 4523, 5098, 6452, 5947, 6150, 5694, 6498, 7048, 7519, 6820, 5652, 6608, 7409, 8729, 10569, 10760, 9054, 9856, 8656, 7972, 9320, 6958, 6820, 7391, 7702, 8248, 9426, 8812, 8666, 8838, 7943, 6878, 7233, 7536, 8381, 8314, 7267, 6704, 7343, 6321, 6409, 6023, 7334, 7975, 7659, 6159, 5990, 6187, 6645, 6702, 6273, 7196, 7381, 6939, 4201, 4108, 5338, 6469, 4528, 3679, 4113, 4158, 3428, 2966, 3466, 3704, 3220, 2582, 2818, 3039, 2835, 1929, 1362, 890, 396, -201, -992, -1502, -2009, -1667, -1503, -1881, -2713, -3231, -2856, -2868, -2989, -4140, -4878, -4690, -3838, -4244, -5312, -9966, -6514, -5246, -4559, -4832, -6833, -8869, -9207, -8021, -7959, -9219, -10911, -12606, -12296, -11710, -10460, -10827, -13095, -12183, -10989, -9458, -9520, -10622, -12221, -11792, -9510, -7964, -7935, -8728, -9137, -8076, -6628, -6379, -7132, -8076, -7499, -6536, -5855, -6285, -7310, -7517, -7217, -6997, -6440, -5806, -4647, -4006, -4144, -3800, -2820, -1811, 215, 768, 531, 186, 514, 2117, 2618, 2396, 1600, 1477, 1800, 2329, 2015, 1585, 1461
};
static float k_gain, r_noise, q_noise, x_est, p_err, z_measure;
void Kalman_Init(void)
{
p_err = 1.0;
r_noise = 10.0;
q_noise = 1;
x_est = -200.0;
p_err = p_err + q_noise;
}
float Kalman_Update(float measure)
{
k_gain = p_err / (p_err + r_noise);
x_est = x_est + k_gain * (measure - x_est);
p_err = (1 - k_gain) * p_err;
p_err = p_err + q_noise;
return x_est;
}
int main(int argc, char *const argv[])
{
int i;
float estimate, new_measure;
Kalman_Init();
for (i = 0; i < sizeof(measures)/sizeof(int); i++)
{
estimate = Kalman_Update((float)measures[i]);
printf("%3d: %6d %10.5f %10.5f %10.5f\r\n", i, measures[i], k_gain, p_err, estimate);
}
return 0;
}
对参数的说明
measures数值来自于手持物体旋转时陀螺仪传感器的真实读数, 本例中陀螺仪的实测噪声\(R\)在10至20这个数量级, 运动中的抖动来源于手持产生的抖动p_err = 1.0;和x_est = -200.0;, 预测和误差的初始值可以随意取一个接近的值, 如果不知道取什么值, 设为0也问题不大.r_noise = 10.0;和q_noise = 1;这两个值会显著影响结果, 其中r_noise可以使用传感器收集静止状态数据后计算方差得到,q_noise无法明确计算, 初始可以赋0.1至1之间的数.
输出结果格式
0: -269 0.04762 0.97619 -12.80952
1: -255 0.08894 1.38937 -34.34924
2: -130 0.12199 1.71988 -46.01752
3: 228 0.14675 1.96749 -5.80566
4: -437 0.16440 2.14403 -76.69533
5: -1234 0.17655 2.26550 -281.01764
6: 1247 0.18471 2.34705 1.21512
7: 173 0.19009 2.40090 33.86971
8: -400 0.19361 2.43607 -50.13048
9: -1561 0.19589 2.45887 -346.09082
10: -1038 0.19736 2.47359 -482.64551
11: 207 0.19831 2.48306 -345.88443
12: 958 0.19891 2.48915 -86.52280
13: -516 0.19930 2.49305 -172.11964
14: -581 0.19955 2.49555 -253.71368
15: -716 0.19971 2.49715 -346.03918
16: -18 0.19982 2.49818 -280.49121
17: -1193 0.19988 2.49883 -462.88641
...
使用不同的 r_noise 和 q_noise 得到的变化曲线如图
图中变化最剧烈的蓝色曲线是从传感器得到的原始测量值, 可以看到原始数据的抖动是很明显的, 经过卡尔曼滤波后可以明显消除抖变, 使结果数据更平滑.
通过变换多种噪声组合, 可以观察到的现象有
- 在
r_noise和q_noise不变的情况下, 不管p_err和x_est设置什么初始值, 都会很快收敛, 最后输出相同的结果序列(这点没有在本例体现, 需要自己验证) r_noise越大表示测量噪声越大, 测量值的权重越低,r_noise越大, 结果越平滑但是延迟也越大q_noise是系统的固有误差,q_noise越小, 结果越平滑延迟越大r_noise和q_noise等比例变化时, 产生的结果序列不变, 图中 r=10,q=0.5 和 r=20,q=1 这两个曲线是重合的.
总结
从卡尔曼滤波器的定义看
- 整个过程中, 对状态 \(\hat{x}\) 的预测和更新, 除了自身和观测值\(z_t\)之外, 关系到这几个参数 \(F_t, B_t, u_t, K_t, H_t\), 其中 \(F_t, u_t, H_t\) 在系统中都相对固定, 而 \(B_t\) 是已知输入, 例如电机或舵机的动作, 已知且确定的, 不存在噪声, 真正起作用的是 \(K_t\) 这个参数.
- 而 \(K_t\) 这个参数的计算, 和 \(\hat{x}\) 没关系. 系统中不存在反馈, 观测值 \(z_t\) 和预测值 \(\hat{x}\) 都不会影响 \(K_t\), 只要 \(H_t, R_t, Q_t\) 这三个值固定, 最后 \(K_t\) 会收敛为一个常量
当符合上面两点条件时, 状态的更新公式就变成下面的式子
\(\hat{x_{t|t}} = \hat{x_{t|t-1}} + K_t (z_t - \hat{x_{t|t-1}}) \\ = \hat{x_{t|t-1}} + K_t z_t - K_t \hat{x_{t|t-1}} \\ = (1 - K_t) \hat{x_{t|t-1}} + K_t z_t\)
令 \(\beta = 1 - K_t\), 这就是一个典型的离散序列差分方程(difference equation)构成的低通滤波器
\(\hat{x_{t|t}} = \beta \hat{x_{t|t-1}} + (1 - \beta) z_t\)
在实际使用中, \(Q\)和\(R\)大概率是常数, 增益\(K_t\)会快速收敛, 上面的式子更简单, 更容易理解和实现, 也符合它的典型使用方式, 即手动调整\(\beta\) (等价于调整\(Q\)和\(R\)), 在延迟和平滑之间找到最佳平衡.
