用python编写的UKF(无迹卡尔曼滤波)代码,状态量和观测量都是三维、非线性的,注释使用中文
import numpy as np
class UKF:
def __init__(self, dim_x, dim_z, alpha=1e-3, beta=2, kappa=0):
self.dim_x = dim_x # 状态维度
self.dim_z = dim_z # 观测维度
self.alpha = alpha # 探测精度
self.beta = beta # 统计量
self.kappa = kappa # 二次调节参数
# 计算λ和权重
self.lam = alpha**2 * (dim_x + kappa) - dim_x
self.gamma = np.sqrt(dim_x + self.lam) # sigma点扩展因子
# 权重
self.Wm = np.full(2 * dim_x + 1, 0.5 / (dim_x + self.lam))
self.Wm[0] = self.lam / (dim_x + self.lam) # 第一个sigma点的权重
self.Wc = np.copy(self.Wm)
self.Wc[0] += (1 - alpha**2 + beta) # 计算协方差权重
self.x = np.zeros(dim_x) # 初始状态
self.P = np.eye(dim_x) # 初始协方差
def predict(self, dt):
# 生成sigma点
sigma_points = self._sigma_points(self.x, self.P)
# 状态转移函数 (非线性)
for i in range(sigma_points.shape[0]):
sigma_points[i] = self._state_transition(sigma_points[i], dt)
# 计算预测状态和协方差
self.x = np.dot(self.Wm, sigma_points) # 预测状态
self.P = self._update_covariance(sigma_points, self.x)
def update(self, z):
# 生成sigma点
sigma_points = self._sigma_points(self.x, self.P)
# 观测预测 (非线性)
z_pred = np.zeros((2 * self.dim_x + 1, self.dim_z))
for i in range(sigma_points.shape[0]):
z_pred[i] = self._measurement_function(sigma_points[i])
# 计算观测均值和协方差
z_mean = np.dot(self.Wm, z_pred) # 观测均值
P_zz = self._update_covariance(z_pred, z_mean) # 观测协方差
# 计算交叉协方差
P_xz = np.zeros((self.dim_x, self.dim_z))
for i in range(sigma_points.shape[0]):
P_xz += self.Wc[i] * np.outer(sigma_points[i] - self.x, z_pred[i] - z_mean)
# 卡尔曼增益
K = np.dot(P_xz, np.linalg.inv(P_zz))
# 更新状态和协方差
self.x += np.dot(K, z - z_mean)
self.P -= np.dot(K, P_zz).dot(K.T)
def _sigma_points(self, x, P):
"""生成sigma点"""
n = self.dim_x
sigma_points = np.zeros((2 * n + 1, n))
sigma_points[0] = x
A = np.linalg.cholesky((n + self.lam) * P) # Cholesky分解
for i in range(n):
sigma_points[i + 1] = x + A[i]
sigma_points[n + i + 1] = x - A[i]
return sigma_points
def _state_transition(self, x, dt):
"""状态转移函数"""
# 示例:简单的非线性运动模型
F = np.array([[1, dt, 0],
[0, 1, dt],
[0, 0, 1]])
return F @ x # 状态转移
def _measurement_function(self, x):
"""观测模型"""
# 示例:简单的非线性观测模型
H = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
return H @ x # 观测
def _update_covariance(self, sigma_points, mean):
"""更新协方差"""
P = np.zeros((self.dim_x, self.dim_x))
for i in range(sigma_points.shape[0]):
P += self.Wc[i] * np.outer(sigma_points[i] - mean, sigma_points[i] - mean)
return P
# 示例用法
if __name__ == "__main__":
dt = 0.1 # 时间步长
ukf = UKF(dim_x=3, dim_z=3) # 初始化UKF
# 模拟预测和更新
for _ in range(100):
ukf.predict(dt) # 预测步骤
# 假设有某个观测值
z = np.array([1, 0.5, 0.3]) # 示例观测值
ukf.update(z) # 更新步骤
print("当前状态:", ukf.x)
这段代码实现了无迹卡尔曼滤波器(UKF),适用于非线性系统的状态估计。以下是对代码的详细讲解和相关公式的说明:
初始化参数:
dim_x
: 状态空间的维度。dim_z
: 观测空间的维度。alpha
: 控制 sigma 点的散布程度,通常取小值(如 1e-3)。beta
: 用于结合先验分布的统计信息,通常取 2。kappa
: 二次调节参数,通常取 0。计算参数:
λ
(lambda)和权重 Wm
、Wc
,用于生成和加权 sigma 点。λ
的计算公式为:Wm
和 Wc
的计算公式为:生成 sigma 点:
状态转移:
计算预测状态和协方差:
生成 sigma 点:
观测预测:
计算观测均值和协方差:
交叉协方差:
卡尔曼增益:
更新状态和协方差:
代码最后部分展示了如何使用 UKF 类进行状态预测和更新。通过循环模拟 100 次预测和更新,在每次更新中使用示例观测值。
无迹卡尔曼滤波器适用于处理非线性系统,通过生成和加权 sigma 点来有效捕捉状态的分布信息。这段代码提供了一个基本的 UKF 实现,涵盖了状态预测、观测更新和协方差更新的完整流程。
如需帮助,或有导航、定位滤波相关的代码定制需求,请点击下方卡片联系作者