尝试一个简单的卡尔曼滤波

最近翻看了这篇翻译的文章详解卡尔曼滤波原理-CSDN博客

萌发了找个简单例子帮助自己理解的想法, 卡尔曼的作用在于可以在任何含有不确定信息 的动态系统中使用滤波,对系统下一步的走向做出有根据的预测,即使伴随着各种干扰,卡尔曼滤波总是能指出真实发生的情况。

用一个线性随机波动的真实序列做卡尔曼滤波测试

复制代码
import numpy as np
import matplotlib.pyplot as plt

# 定义卡尔曼滤波器
class KalmanFilter:
    def __init__(self, process_variance, measurement_variance, estimated_measurement_variance):
        self.process_variance = process_variance
        self.measurement_variance = measurement_variance
        self.estimated_measurement_variance = estimated_measurement_variance
        self.posteri_estimate = 0.0
        self.posteri_error_estimate = 1.0

    def update(self, measurement):
        # 预测步骤
        priori_estimate = self.posteri_estimate
        priori_error_estimate = self.posteri_error_estimate + self.process_variance

        # 更新步骤
        blending_factor = priori_error_estimate / (priori_error_estimate + self.measurement_variance)
        self.posteri_estimate = priori_estimate + blending_factor * (measurement - priori_estimate)
        self.posteri_error_estimate = (1 - blending_factor) * priori_error_estimate

        return self.posteri_estimate

# 模拟观测数据
np.random.seed(0)
true_values = np.linspace(0, 10, 100) + np.random.normal(0, 1, 100)

# 初始化滤波器
kf = KalmanFilter(process_variance=0.01, measurement_variance=1, estimated_measurement_variance=0.01)

# 对观测数据进行滤波
filtered_values = []
for measurement in true_values:
    filtered_values.append(kf.update(measurement))

# 绘制结果
plt.plot(range(100), true_values, label='True Values')
plt.plot(range(100), filtered_values, label='Filtered Values')
plt.xlabel('Time')
plt.ylabel('Value')
plt.title('Kalman Filter Demo')
plt.legend()
plt.show()

类的构造函数,用于初始化卡尔曼滤波器的参数。

  • process_variance:过程噪声的方差,表示系统自身的变化不确定性。
  • measurement_variance:测量噪声的方差,表示测量值的不确定性。
  • estimated_measurement_variance:预估的测量方差(在这个代码中未使用)。
  • posteri_estimate:后验估计值,初始化为 0.0,表示当前对系统状态的估计。
  • posteri_error_estimate:后验误差估计,初始化为 1.0,表示当前估计的误差。

update 方法:用于更新卡尔曼滤波器的状态。

  • measurement:当前测量值。
  • 预测步骤
    • priori_estimate:当前的后验估计值,即上一次更新后的估计。
    • priori_error_estimate:预测的误差估计,通过将后验误差加上过程噪声来获得。

更新步骤

  • blending_factor:混合因子,决定了预测值和测量值在最终估计中的权重。它的值在 0 到 1 之间,越接近 1 表示测量值越可信,越接近 0 表示预测值越可信。
  • self.posteri_estimate:更新后的后验估计值,通过将预测值与当前测量值结合来计算。
  • self.posteri_error_estimate:更新后的后验误差估计,反映了新的误差情况。

最后,该方法返回更新后的后验估计值,表示当前对系统状态的最新估计。

整体来说,KalmanFilter 类实现了卡尔曼滤波的基本逻辑,能够根据不断更新的测量值来优化对系统状态的估计。通过预测和更新步骤,它有效地结合了测量噪声和过程噪声,从而提供一个更准确的状态估计。

测试结果

相关推荐
上单带刀不带妹40 分钟前
手写 Vue 中虚拟 DOM 到真实 DOM 的完整过程
开发语言·前端·javascript·vue.js·前端框架
im_AMBER1 小时前
学习日志05 python
python·学习
大虫小呓1 小时前
Python 处理 Excel 数据 pandas 和 openpyxl 哪家强?
python·pandas
哪 吒1 小时前
2025B卷 - 华为OD机试七日集训第5期 - 按算法分类,由易到难,循序渐进,玩转OD(Python/JS/C/C++)
python·算法·华为od·华为od机试·2025b卷
-凌凌漆-1 小时前
【Qt】QStringLiteral 介绍
开发语言·qt
程序员爱钓鱼1 小时前
Go语言项目工程化 — 常见开发工具与 CI/CD 支持
开发语言·后端·golang·gin
军训猫猫头2 小时前
1.如何对多个控件进行高效的绑定 C#例子 WPF例子
开发语言·算法·c#·.net
真的想上岸啊2 小时前
学习C++、QT---18(C++ 记事本项目的stylesheet)
开发语言·c++·学习
明天好,会的2 小时前
跨平台ZeroMQ:在Rust中使用zmq库的完整指南
开发语言·后端·rust
摸爬滚打李上进2 小时前
重生学AI第十六集:线性层nn.Linear
人工智能·pytorch·python·神经网络·机器学习