卡尔曼滤波器的定义,实例和代码实现

原创已于 2024-03-03 02:00:25 修改 · 1.5k 阅读

31 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #卡尔曼滤波

于 2024-02-28 19:31:07 首次发布

Embeded 同时被 2 个专栏收录

196 篇文章

订阅专栏

Algorithm

27 篇文章

订阅专栏

本文介绍卡尔曼滤波器，它能从含噪测量值中估计动态系统状态，适合嵌入式系统。阐述其计算步骤，给出完整定义及简化公式，通过实例展示计算过程，用C语言代码演示实现，最后总结其本质是低通滤波器，可手动调整参数平衡延迟和平滑。

卡尔曼滤波器(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$
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}$

对以上符号的说明

$\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$
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}$

实例

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$

卡尔曼增益

$\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$

卡尔曼增益

$\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$