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

这篇具有很好参考价值的文章主要介绍了卡尔曼滤波器的定义,实例和代码实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

卡尔曼滤波器(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误差和预测值.

#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_noiseq_noise 得到的变化曲线如图

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

图中变化最剧烈的蓝色曲线是从传感器得到的原始测量值, 可以看到原始数据的抖动是很明显的, 经过卡尔曼滤波后可以明显消除抖变, 使结果数据更平滑.

通过变换多种噪声组合, 可以观察到的现象有

  1. r_noiseq_noise 不变的情况下, 不管 p_errx_est 设置什么初始值, 都会很快收敛, 最后输出相同的结果序列(这点没有在本例体现, 需要自己验证)
  2. r_noise越大表示测量噪声越大, 测量值的权重越低, r_noise越大, 结果越平滑但是延迟也越大
  3. q_noise是系统的固有误差, q_noise越小, 结果越平滑延迟越大
  4. r_noiseq_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\)), 在延迟和平滑之间找到最佳平衡.文章来源地址https://www.toymoban.com/news/detail-837749.html


参考

  • 卡尔曼滤波 非常好的介绍 https://www.kalmanfilter.net/CN/alphabeta_cn.html
  • 扩展卡尔曼滤波 https://simondlevy.github.io/ekf-tutorial/
  • 概念说明和Python实现 https://forecastegy.com/posts/kalman-filter-for-time-series-forecasting-in-python/
  • 另一篇浅显易懂的卡尔曼滤波器说明 https://thekalmanfilter.com/kalman-filter-explained-simply/

到了这里,关于卡尔曼滤波器的定义,实例和代码实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 卡尔曼滤波器(目标跟踪一)(上)

    本文主要是针对目标跟踪算法进行一个学习编码,从比较简单的卡尔曼滤波器开始,到后面的deepsort 和最后与yolo算法进行整合,到最后手动实现目标跟踪框架的流程进行。本着,无法造轮子就没有彻底理解的原则进行学习。那么废话不多说开始了。(收藏点赞?VIP:Free,白嫖

    2024年02月08日
    浏览(49)
  • 【图像处理 】卡尔曼滤波器原理

    目录 一、说明 二、它是什么? 2.1 我们可以用卡尔曼滤波器做什么? 2.2 卡尔曼滤波器如何看待您的问题

    2024年02月06日
    浏览(48)
  • 【状态估计】基于卡尔曼滤波器和扩展卡尔曼滤波器用于 INS/GNSS 导航、目标跟踪和地形参考导航研究(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果  2.1 算例1 ​ 2.2 算例2  2.3 算例3 🎉3 参

    2024年02月11日
    浏览(48)
  • 卡尔曼滤波器原理讲解及其matlab实现

    目录 一:卡尔曼滤波器的信号模型[1-2] 二:其他方程及变量介绍 三:卡尔曼滤波器递推公式 四:matlab仿真[3] 参考文献: 引言:在进行一些信号处理的过程中,我们通常会采集到一些数据,但是实际测量到的数据是受到噪声干扰了之后的,故与真实的数据有一些偏差。因此

    2023年04月08日
    浏览(42)
  • 【状态估计】粒子滤波器、Σ点滤波器和扩展/线性卡尔曼滤波器研究(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 2.1 扩展卡尔曼滤波 2.2 线性卡尔曼滤波 

    2024年02月09日
    浏览(40)
  • 1. 简明误差卡尔曼滤波器(ESKF)及其推导过程

    本文主要介绍一种特殊正交群 SO(3) text{SO(3)} SO(3) 上的 ESKF(Error State Kalman Filter, 误差卡尔曼滤波器) (有时也叫做 流形上的ESKF )推导过程。 在现代的大多数 IMU 系统中,人们往往使用 误差状态卡尔曼滤波器(Error State Kalman Filter, ESKF) ,而非 原始状态的卡尔曼滤波器 。大部

    2024年02月06日
    浏览(71)
  • 【状态估计】基于线性卡尔曼滤波器和粒子滤波器无人机估计地形高度(Matlab代码实现)

      💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码实现 本文模

    2024年02月16日
    浏览(42)
  • 了解卡尔曼滤波器4--非线性状态估算器(EKF,UKF,PF)

            一般来说,我们希望我们的生活是线性的,就像这条线,这可能表示成功、收入或者幸福。但实际上,生活并不是线性的,它充满了起伏,有时甚至更复杂。         如果您是工程师,您经常会需要处理非线性系统,为了帮助您,我们将讨论非线性状态估算器

    2023年04月20日
    浏览(51)
  • 基于自适应扩展卡尔曼滤波器(AEKF)的锂离子电池SOC估计(附MATLAB代码)

    AEKF_SOC_Estimation函数使用二阶RC等效电路模型(ECM)和自适应扩展卡尔曼滤波器(AEKF)估计电池的端电压(Vt)和充电状态(SOC)。该函数将以下内容作为输入:  · 电流(A) · 电压(V) · 温度(℃) 该函数的输出为: ·  估计SOC · 估计电压Vt · 电压Vt误差 加载电池模型参数以及不

    2023年04月23日
    浏览(38)
  • 使用环境中的视觉地标和扩展卡尔曼滤波器定位移动机器人研究(Matlab代码实现)

     💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码及文章 本文

    2024年02月10日
    浏览(51)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包