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

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

相关文章

C#使用SQLite进行大数据量高效处理的代码示例

《C#使用SQLite进行大数据量高效处理的代码示例》在软件开发中,高效处理大数据量是一个常见且具有挑战性的任务,SQLite因其零配置、嵌入式、跨平台的特性,成为许多开发者的首选数据库,本文将深入探... 目录前言准备工作数据实体核心技术批量插入:从乌龟到猎豹的蜕变分页查询:加载百万数据异步处理:拒绝界面

用js控制视频播放进度基本示例代码

《用js控制视频播放进度基本示例代码》写前端的时候,很多的时候是需要支持要网页视频播放的功能,下面这篇文章主要给大家介绍了关于用js控制视频播放进度的相关资料,文中通过代码介绍的非常详细,需要的朋友可... 目录前言html部分:JavaScript部分:注意:总结前言在javascript中控制视频播放

Java中StopWatch的使用示例详解

《Java中StopWatch的使用示例详解》stopWatch是org.springframework.util包下的一个工具类,使用它可直观的输出代码执行耗时,以及执行时间百分比,这篇文章主要介绍... 目录stopWatch 是org.springframework.util 包下的一个工具类,使用它

Spring Boot 3.4.3 基于 Spring WebFlux 实现 SSE 功能(代码示例)

《SpringBoot3.4.3基于SpringWebFlux实现SSE功能(代码示例)》SpringBoot3.4.3结合SpringWebFlux实现SSE功能,为实时数据推送提供... 目录1. SSE 简介1.1 什么是 SSE?1.2 SSE 的优点1.3 适用场景2. Spring WebFlu

springboot security快速使用示例详解

《springbootsecurity快速使用示例详解》:本文主要介绍springbootsecurity快速使用示例,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝... 目录创www.chinasem.cn建spring boot项目生成脚手架配置依赖接口示例代码项目结构启用s

Python实现无痛修改第三方库源码的方法详解

《Python实现无痛修改第三方库源码的方法详解》很多时候,我们下载的第三方库是不会有需求不满足的情况,但也有极少的情况,第三方库没有兼顾到需求,本文将介绍几个修改源码的操作,大家可以根据需求进行选择... 目录需求不符合模拟示例 1. 修改源文件2. 继承修改3. 猴子补丁4. 追踪局部变量需求不符合很

golang 日志log与logrus示例详解

《golang日志log与logrus示例详解》log是Go语言标准库中一个简单的日志库,本文给大家介绍golang日志log与logrus示例详解,感兴趣的朋友一起看看吧... 目录一、Go 标准库 log 详解1. 功能特点2. 常用函数3. 示例代码4. 优势和局限二、第三方库 logrus 详解1.

idea中创建新类时自动添加注释的实现

《idea中创建新类时自动添加注释的实现》在每次使用idea创建一个新类时,过了一段时间发现看不懂这个类是用来干嘛的,为了解决这个问题,我们可以设置在创建一个新类时自动添加注释,帮助我们理解这个类的用... 目录前言:详细操作:步骤一:点击上方的 文件(File),点击&nbmyHIgsp;设置(Setti

SpringBoot实现MD5加盐算法的示例代码

《SpringBoot实现MD5加盐算法的示例代码》加盐算法是一种用于增强密码安全性的技术,本文主要介绍了SpringBoot实现MD5加盐算法的示例代码,文中通过示例代码介绍的非常详细,对大家的学习... 目录一、什么是加盐算法二、如何实现加盐算法2.1 加盐算法代码实现2.2 注册页面中进行密码加盐2.

在C#中调用Python代码的两种实现方式

《在C#中调用Python代码的两种实现方式》:本文主要介绍在C#中调用Python代码的两种实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录C#调用python代码的方式1. 使用 Python.NET2. 使用外部进程调用 Python 脚本总结C#调