卡尔曼滤波源码注释和调用示例

2024-06-16 16:28

本文主要是介绍卡尔曼滤波源码注释和调用示例,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

卡尔曼滤波源码注释和调用示例

flyfish

Python版本代码地址
C++版代码地址

主要用于分析代码,增加了中文注释

import numpy as np
import scipy.linalg"""
0.95分位数的卡方分布表,N自由度(包含N=1到9的值)。
取自MATLAB/Octave的chi2inv函数,用作Mahalanobis门限。
"""
chi2inv95 = {1: 3.8415,2: 5.9915,3: 7.8147,4: 9.4877,5: 11.070,6: 12.592,7: 14.067,8: 15.507,9: 16.919
}class KalmanFilter(object):"""一个用于图像空间中跟踪边界框的简单卡尔曼滤波器。8维状态空间x, y, a, h, vx, vy, va, vh包含边界框中心位置 (x, y)、长宽比 a、高度 h 及其相应的速度。对象运动遵循恒定速度模型。边界框位置 (x, y, a, h) 被作为状态空间的直接观测值(线性观测模型)。"""def __init__(self):ndim, dt = 4, 1.# 创建卡尔曼滤波器模型矩阵self._motion_mat = np.eye(2 * ndim, 2 * ndim)for i in range(ndim):self._motion_mat[i, ndim + i] = dtself._update_mat = np.eye(ndim, 2 * ndim)# 运动和观测不确定性相对于当前状态估计进行选择。这些权重控制模型中的不确定性量。这有点hacky。self._std_weight_position = 1. / 20self._std_weight_velocity = 1. / 160def initiate(self, measurement):"""从未关联的测量创建跟踪。参数----------measurement : ndarray边界框坐标 (x, y, a, h) 包含中心位置 (x, y)、长宽比 a 和高度 h。返回值-------(ndarray, ndarray)返回新跟踪的均值向量(8维)和协方差矩阵(8x8维)。"""mean_pos = measurementmean_vel = np.zeros_like(mean_pos)mean = np.r_[mean_pos, mean_vel]std = [2 * self._std_weight_position * measurement[3],2 * self._std_weight_position * measurement[3],1e-2,2 * self._std_weight_position * measurement[3],10 * self._std_weight_velocity * measurement[3],10 * self._std_weight_velocity * measurement[3],1e-5,10 * self._std_weight_velocity * measurement[3]]covariance = np.diag(np.square(std))return mean, covariancedef predict(self, mean, covariance):"""基于模型预测下一状态。参数----------mean : ndarray当前状态的均值向量(8维)。covariance : ndarray当前状态的协方差矩阵(8x8维)。返回值-------(ndarray, ndarray)返回预测的均值向量和协方差矩阵。"""std_pos = [self._std_weight_position * mean[3],self._std_weight_position * mean[3],1e-2,self._std_weight_position * mean[3]]std_vel = [self._std_weight_velocity * mean[3],self._std_weight_velocity * mean[3],1e-5,self._std_weight_velocity * mean[3]]motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))mean = np.dot(self._motion_mat, mean)covariance = np.linalg.multi_dot((self._motion_mat, covariance, self._motion_mat.T)) + motion_covreturn mean, covariancedef project(self, mean, covariance):"""将状态分布(均值和协方差)投影到观测空间。参数----------mean : ndarray状态分布的均值向量(8维)。covariance : ndarray状态分布的协方差矩阵(8x8维)。返回值-------(ndarray, ndarray)返回观测空间中的均值向量(4维)和协方差矩阵(4x4维)。"""std = [self._std_weight_position * mean[3],self._std_weight_position * mean[3],1e-1,self._std_weight_position * mean[3]]innovation_cov = np.diag(np.square(std))mean = np.dot(self._update_mat, mean)covariance = np.linalg.multi_dot((self._update_mat, covariance, self._update_mat.T))return mean, covariance + innovation_covdef update(self, mean, covariance, measurement):"""使用观测值更新状态分布。参数----------mean : ndarray先验状态分布的均值向量(8维)。covariance : ndarray先验状态分布的协方差矩阵(8x8维)。measurement : ndarray当前观测到的边界框坐标 (x, y, a, h)。返回值-------(ndarray, ndarray)更新后的状态分布的均值向量和协方差矩阵。"""projected_mean, projected_cov = self.project(mean, covariance)chol_factor, lower = scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)kalman_gain = scipy.linalg.cho_solve((chol_factor, lower),np.dot(covariance, self._update_mat.T).T, check_finite=False).Tinnovation = measurement - projected_meannew_mean = mean + np.dot(innovation, kalman_gain.T)new_covariance = covariance - np.linalg.multi_dot((kalman_gain, projected_cov, kalman_gain.T))return new_mean, new_covariancedef gating_distance(self, mean, covariance, measurements, only_position=False):"""计算状态分布和观测值之间的门限距离。可从 `chi2inv95` 中获得合适的距离门限。如果 `only_position` 为 False,则卡方分布有4个自由度,否则为2个。参数----------mean : ndarray状态分布的均值向量(8维)。covariance : ndarray状态分布的协方差矩阵(8x8维)。measurements : ndarrayN×4维矩阵,包含N个观测值,每个观测值的格式为 (x, y, a, h),其中 (x, y) 为边界框中心位置,a 为长宽比,h 为高度。only_position : 可选[bool]如果为True,距离计算仅针对边界框中心位置。返回值-------ndarray返回长度为N的数组,其中第i个元素包含 (mean, covariance) 和 `measurements[i]` 之间的平方Mahalanobis距离。"""mean, covariance = self.project(mean, covariance)if only_position:mean, covariance = mean[:2], covariance[:2, :2]measurements = measurements[:, :2]cholesky_factor = np.linalg.cholesky(covariance)d = measurements - meanz = scipy.linalg.solve_triangular(cholesky_factor, d.T, lower=True, check_finite=False, overwrite_b=True)squared_maha = np.sum(z * z, axis=0)return squared_maha

调用示例1

import numpy as np
from kalman_filter_cn import KalmanFilterclass KalmanFilterTracker:def __init__(self, initial_measurement):self.kf = KalmanFilter()self.mean, self.covariance = self.kf.initiate(initial_measurement)self.history = [initial_measurement[:2]]  # 只记录位置 (x, y)def predict_and_update(self, measurement):self.mean, self.covariance = self.kf.predict(self.mean, self.covariance)self.mean, self.covariance = self.kf.update(self.mean, self.covariance, measurement)self.history.append(self.mean[:2])  # 只记录位置 (x, y)return self.mean, self.covariance# 示例用法
initial_measurement = np.array([0, 0, 1, 1])
tracker = KalmanFilterTracker(initial_measurement)measurements = [np.array([1, 1, 1, 1]),np.array([2, 2, 1, 1]),np.array([3, 3, 1, 1]),np.array([4, 4, 1, 1]),np.array([5, 5, 1, 1])
]for measurement in measurements:tracker.predict_and_update(measurement)print("History of positions:", tracker.history)
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimationdef animate_kalman_filter(history):fig, ax = plt.subplots()ax.set_xlim(0, 6)ax.set_ylim(0, 6)line, = ax.plot([], [], 'bo-', label='Kalman Filter')true_line, = ax.plot([], [], 'ro--', label='True Path')def init():line.set_data([], [])true_line.set_data([], [])return line, true_linedef update(frame):x_data = [h[0] for h in history[:frame+1]]y_data = [h[1] for h in history[:frame+1]]line.set_data(x_data, y_data)true_x = [i for i in range(len(history))]true_y = [i for i in range(len(history))]true_line.set_data(true_x, true_y)return line, true_lineani = FuncAnimation(fig, update, frames=len(history), init_func=init, blit=True, repeat=True)ani.save('kalman_filter.gif', writer='imagemagick')plt.legend()plt.show()animate_kalman_filter(tracker.history)

在这里插入图片描述

调用示例2

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from kalman_filter_cn import KalmanFilter
from typing import Tupleclass KalmanFilterTracker:def __init__(self, initial_measurement: np.ndarray) -> None:self.kf = KalmanFilter()self.mean, self.covariance = self.kf.initiate(initial_measurement)self.history = [initial_measurement[:2]]  # 只记录位置 (x, y)def predict_and_update(self, measurement: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:self.mean, self.covariance = self.kf.predict(self.mean, self.covariance)self.mean, self.covariance = self.kf.update(self.mean, self.covariance, measurement)self.history.append(self.mean[:2])  # 只记录位置 (x, y)return self.mean, self.covarianceclass KalmanFilterAnimation:def __init__(self, tracker: KalmanFilterTracker, measurements: np.ndarray) -> None:self.tracker = trackerself.measurements = measurementsdef init(self):self.line.set_data([], [])self.true_line.set_data([], [])return self.line, self.true_linedef update(self, frame):x_data = [h[0] for h in self.tracker.history[:frame+1]]y_data = [h[1] for h in self.tracker.history[:frame+1]]self.line.set_data(x_data, y_data)true_x = [m[0] for m in self.measurements[:frame+1]]true_y = [m[1] for m in self.measurements[:frame+1]]self.true_line.set_data(true_x, true_y)return self.line, self.true_linedef animate(self) -> None:fig, ax = plt.subplots()ax.set_xlim(0, 10)ax.set_ylim(-1.5, 1.5)self.line, = ax.plot([], [], 'bo-', label='Kalman Filter')self.true_line, = ax.plot([], [], 'ro--', label='True Path')ani = FuncAnimation(fig, self.update, frames=len(self.tracker.history),init_func=self.init, blit=True, repeat=True)ani.save('kalman_filter_curve.gif', writer='imagemagick')plt.legend()plt.show()# 初始化卡尔曼滤波器
initial_measurement = np.array([0, 0, 1, 1])
tracker = KalmanFilterTracker(initial_measurement)# 生成测量值,形成曲线轨迹(正弦波)
measurements = []
for t in np.linspace(0, 10, 100):x = ty = np.sin(t)measurements.append(np.array([x, y, 1, 1]))# 更新卡尔曼滤波器
for measurement in measurements:tracker.predict_and_update(measurement)# 创建动画并生成GIF
animation = KalmanFilterAnimation(tracker, measurements)
animation.animate()

请添加图片描述
请添加图片描述

如果要分析滤波器性能、调试滤波器以及可视化滤波器是非常有用的,那么可以这样做

class KalmanFilterTracker:def __init__(self, initial_measurement: np.ndarray) -> None:self.kf = KalmanFilter()self.mean, self.covariance = self.kf.initiate(initial_measurement)self.history = [initial_measurement[:2]]  # 只记录位置 (x, y)self.states = [self.mean]  # 存储历史状态均值self.covariances = [self.covariance]  # 存储历史协方差矩阵def predict_and_update(self, measurement: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:self.mean, self.covariance = self.kf.predict(self.mean, self.covariance)self.mean, self.covariance = self.kf.update(self.mean, self.covariance, measurement)self.history.append(self.mean[:2])  # 只记录位置 (x, y)self.states.append(self.mean)  # 存储历史状态均值self.covariances.append(self.covariance)  # 存储历史协方差矩阵return self.mean, self.covariance

记录历史值可以分析滤波器的性能,查找和修正可能的问题。对于可视化和演示目的,存储历史值可以让绘制出估计轨迹和实际轨迹,以便直观地比较和展示滤波效果。

如果只是单纯的用,在递归估计中,只需保持前一时刻的状态即可

class KalmanFilterTracker:def __init__(self, initial_measurement: np.ndarray) -> None:self.kf = KalmanFilter()self.mean, self.covariance = self.kf.initiate(initial_measurement)def predict_and_update(self, measurement: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:self.mean, self.covariance = self.kf.predict(self.mean, self.covariance)self.mean, self.covariance = self.kf.update(self.mean, self.covariance, measurement)return self.mean, self.covariance

这篇关于卡尔曼滤波源码注释和调用示例的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1066949

相关文章

pandas中位数填充空值的实现示例

《pandas中位数填充空值的实现示例》中位数填充是一种简单而有效的方法,用于填充数据集中缺失的值,本文就来介绍一下pandas中位数填充空值的实现,具有一定的参考价值,感兴趣的可以了解一下... 目录什么是中位数填充?为什么选择中位数填充?示例数据结果分析完整代码总结在数据分析和机器学习过程中,处理缺失数

Pandas统计每行数据中的空值的方法示例

《Pandas统计每行数据中的空值的方法示例》处理缺失数据(NaN值)是一个非常常见的问题,本文主要介绍了Pandas统计每行数据中的空值的方法示例,具有一定的参考价值,感兴趣的可以了解一下... 目录什么是空值?为什么要统计空值?准备工作创建示例数据统计每行空值数量进一步分析www.chinasem.cn处

利用Python调试串口的示例代码

《利用Python调试串口的示例代码》在嵌入式开发、物联网设备调试过程中,串口通信是最基础的调试手段本文将带你用Python+ttkbootstrap打造一款高颜值、多功能的串口调试助手,需要的可以了... 目录概述:为什么需要专业的串口调试工具项目架构设计1.1 技术栈选型1.2 关键类说明1.3 线程模

Python使用getopt处理命令行参数示例解析(最佳实践)

《Python使用getopt处理命令行参数示例解析(最佳实践)》getopt模块是Python标准库中一个简单但强大的命令行参数处理工具,它特别适合那些需要快速实现基本命令行参数解析的场景,或者需要... 目录为什么需要处理命令行参数?getopt模块基础实际应用示例与其他参数处理方式的比较常见问http

Android实现在线预览office文档的示例详解

《Android实现在线预览office文档的示例详解》在移动端展示在线Office文档(如Word、Excel、PPT)是一项常见需求,这篇文章为大家重点介绍了两种方案的实现方法,希望对大家有一定的... 目录一、项目概述二、相关技术知识三、实现思路3.1 方案一:WebView + Office Onl

Mysql用户授权(GRANT)语法及示例解读

《Mysql用户授权(GRANT)语法及示例解读》:本文主要介绍Mysql用户授权(GRANT)语法及示例,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录mysql用户授权(GRANT)语法授予用户权限语法GRANT语句中的<权限类型>的使用WITH GRANT

Java中Scanner的用法示例小结

《Java中Scanner的用法示例小结》有时候我们在编写代码的时候可能会使用输入和输出,那Java也有自己的输入和输出,今天我们来探究一下,对JavaScanner用法相关知识感兴趣的朋友一起看看吧... 目录前言一 输出二 输入Scanner的使用多组输入三 综合练习:猜数字游戏猜数字前言有时候我们在

html5的响应式布局的方法示例详解

《html5的响应式布局的方法示例详解》:本文主要介绍了HTML5中使用媒体查询和Flexbox进行响应式布局的方法,简要介绍了CSSGrid布局的基础知识和如何实现自动换行的网格布局,详细内容请阅读本文,希望能对你有所帮助... 一 使用媒体查询响应式布局        使用的参数@media这是常用的

Java使用SLF4J记录不同级别日志的示例详解

《Java使用SLF4J记录不同级别日志的示例详解》SLF4J是一个简单的日志门面,它允许在运行时选择不同的日志实现,这篇文章主要为大家详细介绍了如何使用SLF4J记录不同级别日志,感兴趣的可以了解下... 目录一、SLF4J简介二、添加依赖三、配置Logback四、记录不同级别的日志五、总结一、SLF4J

Java 正则表达式URL 匹配与源码全解析

《Java正则表达式URL匹配与源码全解析》在Web应用开发中,我们经常需要对URL进行格式验证,今天我们结合Java的Pattern和Matcher类,深入理解正则表达式在实际应用中... 目录1.正则表达式分解:2. 添加域名匹配 (2)3. 添加路径和查询参数匹配 (3) 4. 最终优化版本5.设计思