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

最近翻看了这篇翻译的文章详解卡尔曼滤波原理-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 类实现了卡尔曼滤波的基本逻辑,能够根据不断更新的测量值来优化对系统状态的估计。通过预测和更新步骤,它有效地结合了测量噪声和过程噪声,从而提供一个更准确的状态估计。

测试结果

相关推荐
Msshu1232 分钟前
诱骗协议芯片支持PD2.0/3.0/3.1/PPS协议,支持使用一个Type-C与电脑传输数据和快充取电功能
c语言·开发语言·电脑
萧鼎2 分钟前
RAGFlow:构建高效检索增强生成流程的技术解析
人工智能·python
cooljser11 分钟前
告别手动操作!用脚本搞定小程序签到的全过程
python
凌叁儿27 分钟前
从零开始搭建Django博客①--正式开始前的准备工作
python·django·sqlite
景天科技苑42 分钟前
【Rust结构体】Rust结构体详解:从基础到高级应用
开发语言·后端·rust·结构体·关联函数·rust结构体·结构体方法
倔强的石头1061 小时前
【C++指南】位运算知识详解
java·开发语言·c++
攻城狮7号1 小时前
Python爬虫第19节-动态渲染页面抓取之Splash使用下篇
开发语言·爬虫·python·python爬虫
天天进步20151 小时前
Python项目--基于计算机视觉的手势识别控制系统
开发语言·python·计算机视觉
mozun20201 小时前
QT:Qt5 串口模块 (QSerialPort) 在 VS2015 中正确关闭串口避免被占用
开发语言·c++·qt·串口·串口调试·上位机软件
小刘同学++2 小时前
Qt 中线程使用
开发语言·qt