### 无迹卡尔曼滤波 (UKF) 的 Python 和 MATLAB 实现 以下是两个简单的可运行示例代码，分别使用 Python 和 MATLAB 来实现无迹卡尔曼滤波 (Unscented Kalman Filter, UKF)，以便用户可以直接测试其功能。 --- #### **Python 实现** 下面是一个基于 `numpy` 库的简单 UKF 示例代码： ```python import numpy as np class UnscentedKalmanFilter: def __init__(self, state_dim, obs_dim, dt): self.state_dim = state_dim self.obs_dim = obs_dim self.dt = dt # 初始化状态向量和协方差矩阵 self.x = np.zeros((state_dim, 1)) self.P = np.eye(state_dim) # 过程噪声和测量噪声协方差矩阵 self.Q = np.eye(state_dim) * 0.01 self.R = np.eye(obs_dim) * 0.1 def sigma_points(self, mean, cov, alpha=1e-3, beta=2, kappa=0): n = len(mean) lambda_ = alpha**2 * (n + kappa) - n Wm = np.full(2*n+1, 0.5/(n+lambda_)) Wc = Wm.copy() Wm[0] = lambda_ / (n + lambda_) Wc[0] = Wm[0] + (1 - alpha**2 + beta) sqrt_P = np.linalg.cholesky(cov) points = np.zeros((2*n+1, n)) points[0] = mean.flatten() for i in range(n): points[i+1] = mean.flatten() + np.sqrt(n + lambda_) * sqrt_P[:, i] points[n+i+1] = mean.flatten() - np.sqrt(n + lambda_) * sqrt_P[:, i] return points.T, Wm, Wc def predict(self, f_func=None): if f_func is None: F = np.eye(self.state_dim) + np.array([[0, self.dt], [0, 0]]) Q = self.Q self.x = F @ self.x self.P = F @ self.P @ F.T + Q else: sigmas, Wm, _ = self.sigma_points(self.x, self.P) sigmas_f = np.apply_along_axis(f_func, 0, sigmas).T self.x = np.sum(Wm.reshape(-1, 1) * sigmas_f, axis=0).reshape(-1, 1) Pxx = np.zeros_like(self.P) for i in range(len(sigmas)): err = sigmas_f[:, i].reshape(-1, 1) - self.x Pxx += Wm[i] * err @ err.T self.P = Pxx + self.Q def update(self, z, h_func=None): if h_func is None: H = np.array([[1, 0]]) # 测量矩阵 R = self.R y = z - H @ self.x S = H @ self.P @ H.T + R K = self.P @ H.T @ np.linalg.inv(S) self.x = self.x + K @ y self.P = self.P - K @ H @ self.P else: sigmas, _, Wc = self.sigma_points(self.x, self.P) sigmas_h = np.apply_along_axis(h_func, 0, sigmas).T z_pred = np.sum(Wc.reshape(-1, 1) * sigmas_h, axis=0).reshape(-1, 1) Pzz = np.zeros((self.obs_dim, self.obs_dim)) for i in range(len(sigmas)): err = sigmas_h[:, i].reshape(-1, 1) - z_pred Pzz += Wc[i] * err @ err.T Pzz += self.R Pyz = np.zeros((self.state_dim, self.obs_dim)) for i in range(len(sigmas)): err_x = sigmas[:, i].reshape(-1, 1) - self.x err_z = sigmas_h[:, i].reshape(-1, 1) - z_pred Pyz += Wc[i] * err_x @ err_z.T K = Pyz @ np.linalg.inv(Pzz) self.x = self.x + K @ (z - z_pred) self.P = self.P - K @ Pzz @ K.T # 定义系统参数 dt = 0.1 ukf = UnscentedKalmanFilter(state_dim=2, obs_dim=1, dt=dt) # 模拟真实轨迹 true_state = np.array([0, 1]).reshape(-1, 1) measurements = [] for t in range(100): true_state = true_state + np.array([0, dt]).reshape(-1, 1) + np.random.randn(2, 1) * 0.01 measurement = true_state[0] + np.random.randn() * 0.1 measurements.append(measurement) ukf.predict() ukf.update(np.array([[measurement]])) print("最终估计状态:", ukf.x.flatten()) ``` 此代码实现了基本的 UKF 功能，并通过模拟数据验证了其性能[^3]。 --- #### **MATLAB 实现** 下面是 MATLAB 中的一个简单 UKF 示例代码： ```matlab function ukf_example() % 参数初始化 dt = 0.1; % 时间步长 T = 100; % 总时间步数 x_true = [0; 1]; % 初始真实状态 x_est = [0; 0]; % 初始估计状态 P = eye(2); % 初始误差协方差矩阵 Q = diag([0.01, 0]); % 过程噪声协方差 R = 0.1; % 测量噪声协方差 % 存储结果 estimates = zeros(T, 2); measurements = zeros(T, 1); for k = 1:T % 更新真实状态 x_true = x_true + [0; dt] + randn(2, 1) .* sqrt(diag(Q)); % 获取测量值 z = x_true(1) + randn() * sqrt(R); measurements(k) = z; % 预测阶段 [sigmas, Wm, Wc] = generate_sigma_points(x_est, P); sigmas_f = arrayfun(@(i) motion_model(sigmas(:, i), dt), 1:size(sigmas, 2), 'UniformOutput', false); sigmas_f = cell2mat(sigmas_f'); x_pred = sum(bsxfun(@times, Wm(:)', sigmas_f), 2); P_pred = compute_covariance(sigmas_f, x_pred, Wc, Q); % 更新阶段 sigmas_h = arrayfun(@(i) observation_model(sigmas_f(:, i)), 1:size(sigmas_f, 2), 'UniformOutput', false); sigmas_h = cell2mat(sigmas_h'); z_pred = sum(bsxfun(@times, Wm(:)', sigmas_h), 2); Pzz = compute_covariance(sigmas_h, z_pred, Wc, R); Pxz = compute_cross_covariance(sigmas_f, x_pred, sigmas_h, z_pred, Wc); K = Pxz / Pzz; x_est = x_pred + K * (z - z_pred); P = P_pred - K * Pzz * K'; % 记录估计结果 estimates(k, :) = x_est'; end disp('最终估计状态:'); disp(estimates(end, :)); end % 辅助函数：生成Sigma点 function [sigmas, Wm, Wc] = generate_sigma_points(mu, Sigma, alpha, beta, kappa) if nargin < 5 alpha = 1e-3; beta = 2; kappa = 0; end n = length(mu); lambda_ = alpha^2 * (n + kappa) - n; Wm = [lambda_ / (n + lambda_), repmat(0.5 / (n + lambda_), 1, 2*n)]; Wc = Wm; Wc(1) = Wc(1) + (1 - alpha^2 + beta); L = chol((n + lambda_) * Sigma)'; sigmas = [mu, mu + L, mu - L]; end % 辅助函数：运动模型 function x_next = motion_model(x_curr, dt) x_next = x_curr + [0; dt]; end % 辅助函数：观测模型 function z = observation_model(x) z = x(1); end % 辅助函数：计算协方差 function Cov = compute_covariance(points, mean_point, weights, noise_cov) diffs = bsxfun(@minus, points, mean_point); Cov = diff_weights(weights, diffs) + noise_cov; end % 辅助函数：计算交叉协方差 function CrossCov = compute_cross_covariance(points1, mean1, points2, mean2, weights) diffs1 = bsxfun(@minus, points1, mean1); diffs2 = bsxfun(@minus, points2, mean2); CrossCov = cross_diff_weights(weights, diffs1, diffs2); end % 辅助函数：加权差异平方 function result = diff_weights(weights, diffs) result = zeros(size(diffs, 1), size(diffs, 2)); for i = 1:length(weights) result = result + weights(i) * (diffs(:, i) * diffs(:, i)'); end end % 辅助函数：加权交叉乘积 function result = cross_diff_weights(weights, diffs1, diffs2) result = zeros(size(diffs1, 1), size(diffs2, 2)); for i = 1:length(weights) result = result + weights(i) * (diffs1(:, i) * diffs2(:, i)'); end end ``` 该 MATLAB 脚本展示了如何构建一个完整的 UKF 循环，并提供了辅助函数以简化核心逻辑[^3]。 --- ### 结果分析 以上两种实现均能完成对动态系统的状态估计任务。对于更复杂的场景（如非线性系统或多变量输入），可以根据具体需求调整过程模型和观测模型。 --- ###