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

Milton2024-02-28 19:32

卡尔曼滤波器(Kalman filter)是一种高效的递归滤波器, 能够从一系列包含噪音的测量值中估计动态系统的状态. 因为不需要存储历史状态, 没有复杂计算, 非常适合在资源有限的嵌入式系统中使用. 常用于飞行器的导引, 导航及控制, 机械和金融中的时间序列分析, 轨迹最佳化等. 卡尔曼滤波不需要假设误差是正态分布, 但如果误差属于正态分布, 卡尔曼滤波的结果会更为准确.

卡尔曼滤波的计算分二个步骤: 预测与更新. 在预测阶段, 滤波器基于上一步的预测结果, 预测当前状态和误差; 在更新阶段, 滤波器利用当前的测量值和预测值, 计算得到新的状态值和误差.

  1. Original Error Estimate, calculate the Kalman Gain using Error in Estimate and Error in Data(Measurement)
    预测阶段, 用预测误差和测量误差计算卡尔曼增益
  2. Original Estimate, calculate Current Estimate using Kalman Gain , Previous Estimate and Measured Value
    更新阶段, 结合测量值, 用卡尔曼增益计算当前的状态
  3. 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\)

卡尔曼增益

$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误差和预测值.

c 复制代码 
#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之间的数.

输出结果

milton@somewhere:~$ ./build/kalman 
  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
 18:   -989    0.19993    2.49925 -568.06982
 19:   -593    0.19995    2.49952 -573.05469
 20:    484    0.19997    2.49969 -361.67612
 21:    102    0.19998    2.49980 -268.94998
 22:    718    0.19999    2.49987  -71.57236
 23:   1362    0.19999    2.49992  215.13065
 24:   1563    0.19999    2.49995  484.69757
 25:   2683    0.20000    2.49997  924.35083
 26:    428    0.20000    2.49998  825.08173
 27:   1616    0.20000    2.49999  983.26434
 28:   2922    0.20000    2.49999 1371.00977
 29:   2968    0.20000    2.49999 1690.40698
 30:   3046    0.20000    2.50000 1961.52515
 31:   3572    0.20000    2.50000 2283.61963
 32:   4006    0.20000    2.50000 2628.09546

可以观察到的现象

  1. 在r_noise 和 q_noise 不变的情况下, 不管 p_err 和 x_est 设置什么初始值, 都会很快收敛, 最后输出相同的结果序列
  2. r_noise越大表示测量噪声越大, 测量值的权重越低, r_noise越大, 结果越平滑但是延迟也越大
  3. q_noise是系统的固有误差, q_noise越小, 结果越平滑延迟越大
  4. r_noise 和 q_noise 等比例变化时, 产生的结果序列不变

总结

从卡尔曼滤波器的定义看

  • 整个过程中, 对状态 \(\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\)), 在延迟和平滑之间找到最佳平衡.

参考

热门推荐
01玄机平台应急响应—webshell查杀02Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO03【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总04新版微信小程序获取用户手机号05文件或文件夹名称中有空格如何批量去除06组基轨迹建模 GBTM的介绍与实现（Stata 或 R）07【TC3xx芯片】TC3xx芯片电源管理系统PMS详解08优化手机性能，解决卡顿问题：关闭这3个微信开关，释放内存空间09基于YOLOv10深度学习的CT扫描图像肾结石智能检测系统【python源码+Pyqt5界面+数据集+训练代码】深度学习实战、目标检测10【Flutter】基于 GetX 实现 Dio 的生命周期自动化