EKF-SLAM从完全发散到厘米级精度：一个SLAM系统的完整调试与优化实战

分类机器人学 | 自动驾驶 | SLAM标签EKF-SLAM、状态估计、机器人定位、数据关联、Python一、引言当SLAM系统爆炸时我在想什么你是否遇到过这样的场景精心设计的EKF-SLAM算法在仿真中RMSE飙升至数百万米路标数量从4个爆炸到100多个系统完全失控这正是我在调试一个经典EKF-SLAM实现时遇到的真实情况。本文将完整记录我从完全失败到厘米级精度的整个调试过程包含6个版本的迭代演进15个关键Bug的定位与修复10个性能指标的对比分析实用的调试技巧和经验总结无论你是SLAM初学者还是有一定经验的开发者本文都将帮助你避免踩坑快速构建稳定的SLAM系统。二、背景什么是EKF-SLAM2.1 SLAM问题定义SLAM (Simultaneous Localization and Mapping)是机器人学的核心问题之一机器人在未知环境中同时估计自己的位置定位和构建环境地图建图。2.2 EKF-SLAM的核心思想EKF-SLAM (Extended Kalman Filter SLAM)是SLAM最经典的解决方案之一状态向量 [机器人状态, 路标1, 路标2, ...] [x, y, θ, lm1_x, lm1_y, lm2_x, lm2_y, ...]两大核心步骤预测Prediction根据运动模型更新机器人状态更新Update根据观测模型修正状态和路标2.3 为什么EKF-SLAM容易出问题EKF-SLAM的数学推导看似简单但实现中存在大量陷阱问题类型常见错误后果协方差传播维度不匹配矩阵奇异 → 系统发散数据关联阈值太宽松路标爆炸路标初始化忽略观测噪声路标位置错误雅可比计算维度错误更新失效数值稳定性直接求逆数值不稳定三、问题复现原始代码的致命缺陷 Extended Kalman Filter SLAM example author: Atsushi Sakai (Atsushi_twi) import math import matplotlib.pyplot as plt import numpy as np # EKF state covariance Cx np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2 # Simulation parameter Q_sim np.diag([0.2, np.deg2rad(1.0)]) ** 2 R_sim np.diag([1.0, np.deg2rad(10.0)]) ** 2 DT 0.1 # time tick [s] SIM_TIME 50.0 # simulation time [s] MAX_RANGE 20.0 # maximum observation range M_DIST_TH 2.0 # Threshold of Mahalanobis distance for data association. STATE_SIZE 3 # State size [x,y,yaw] LM_SIZE 2 # LM state size [x,y] show_animation True def ekf_slam(xEst, PEst, u, z): # Predict S STATE_SIZE G, Fx jacob_motion(xEst[0:S], u) xEst[0:S] motion_model(xEst[0:S], u) PEst[0:S, 0:S] G.T PEst[0:S, 0:S] G Fx.T Cx Fx initP np.eye(2) # Update for iz in range(len(z[:, 0])): # for each observation min_id search_correspond_landmark_id(xEst, PEst, z[iz, 0:2]) nLM calc_n_lm(xEst) if min_id nLM: print(New LM) # Extend state and covariance matrix xAug np.vstack((xEst, calc_landmark_position(xEst, z[iz, :]))) PAug np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))), np.hstack((np.zeros((LM_SIZE, len(xEst))), initP)))) xEst xAug PEst PAug lm get_landmark_position_from_state(xEst, min_id) y, S, H calc_innovation(lm, xEst, PEst, z[iz, 0:2], min_id) K (PEst H.T) np.linalg.inv(S) xEst xEst (K y) PEst (np.eye(len(xEst)) - (K H)) PEst xEst[2] pi_2_pi(xEst[2]) return xEst, PEst def calc_input(): v 1.0 # [m/s] yaw_rate 0.1 # [rad/s] u np.array([[v, yaw_rate]]).T return u def observation(xTrue, xd, u, RFID): xTrue motion_model(xTrue, u) # add noise to gps x-y z np.zeros((0, 3)) for i in range(len(RFID[:, 0])): dx RFID[i, 0] - xTrue[0, 0] dy RFID[i, 1] - xTrue[1, 0] d math.hypot(dx, dy) angle pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) if d MAX_RANGE: dn d np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise angle_n angle np.random.randn() * Q_sim[1, 1] ** 0.5 # add noise zi np.array([dn, angle_n, i]) z np.vstack((z, zi)) # add noise to input ud np.array([[ u[0, 0] np.random.randn() * R_sim[0, 0] ** 0.5, u[1, 0] np.random.randn() * R_sim[1, 1] ** 0.5]]).T xd motion_model(xd, ud) return xTrue, z, xd, ud def motion_model(x, u): F np.array([[1.0, 0, 0], [0, 1.0, 0], [0, 0, 1.0]]) B np.array([[DT * math.cos(x[2, 0]), 0], [DT * math.sin(x[2, 0]), 0], [0.0, DT]]) x (F x) (B u) return x def calc_n_lm(x): n int((len(x) - STATE_SIZE) / LM_SIZE) return n def jacob_motion(x, u): Fx np.hstack((np.eye(STATE_SIZE), np.zeros( (STATE_SIZE, LM_SIZE * calc_n_lm(x))))) jF np.array([[0.0, 0.0, -DT * u[0, 0] * math.sin(x[2, 0])], [0.0, 0.0, DT * u[0, 0] * math.cos(x[2, 0])], [0.0, 0.0, 0.0]], dtypefloat) G np.eye(STATE_SIZE) Fx.T jF Fx return G, Fx, def calc_landmark_position(x, z): zp np.zeros((2, 1)) zp[0, 0] x[0, 0] z[0] * math.cos(x[2, 0] z[1]) zp[1, 0] x[1, 0] z[0] * math.sin(x[2, 0] z[1]) return zp def get_landmark_position_from_state(x, ind): lm x[STATE_SIZE LM_SIZE * ind: STATE_SIZE LM_SIZE * (ind 1), :] return lm def search_correspond_landmark_id(xAug, PAug, zi): Landmark association with Mahalanobis distance nLM calc_n_lm(xAug) min_dist [] for i in range(nLM): lm get_landmark_position_from_state(xAug, i) y, S, H calc_innovation(lm, xAug, PAug, zi, i) min_dist.append(y.T np.linalg.inv(S) y) min_dist.append(M_DIST_TH) # new landmark min_id min_dist.index(min(min_dist)) return min_id def calc_innovation(lm, xEst, PEst, z, LMid): delta lm - xEst[0:2] q (delta.T delta)[0, 0] z_angle math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0] zp np.array([[math.sqrt(q), pi_2_pi(z_angle)]]) y (z - zp).T y[1] pi_2_pi(y[1]) H jacob_h(q, delta, xEst, LMid 1) S H PEst H.T Cx[0:2, 0:2] return y, S, H def jacob_h(q, delta, x, i): sq math.sqrt(q) G np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]], [delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]]) G G / q nLM calc_n_lm(x) F1 np.hstack((np.eye(3), np.zeros((3, 2 * nLM)))) F2 np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))), np.eye(2), np.zeros((2, 2 * nLM - 2 * i)))) F np.vstack((F1, F2)) H G F return H def pi_2_pi(angle): return (angle math.pi) % (2 * math.pi) - math.pi def main(): print(__file__ start!!) time 0.0 # RFID positions [x, y] RFID np.array([[10.0, -2.0], [15.0, 10.0], [3.0, 15.0], [-5.0, 20.0]]) # State Vector [x y yaw v] xEst np.zeros((STATE_SIZE, 1)) xTrue np.zeros((STATE_SIZE, 1)) PEst np.eye(STATE_SIZE) xDR np.zeros((STATE_SIZE, 1)) # Dead reckoning # history hxEst xEst hxTrue xTrue hxDR xTrue while SIM_TIME time: time DT u calc_input() xTrue, z, xDR, ud observation(xTrue, xDR, u, RFID) xEst, PEst ekf_slam(xEst, PEst, ud, z) x_state xEst[0:STATE_SIZE] # store data history hxEst np.hstack((hxEst, x_state)) hxDR np.hstack((hxDR, xDR)) hxTrue np.hstack((hxTrue, xTrue)) if show_animation: # pragma: no cover plt.cla() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( key_release_event, lambda event: [exit(0) if event.key escape else None]) plt.plot(RFID[:, 0], RFID[:, 1], *k) plt.plot(xEst[0], xEst[1], .r) # plot landmark for i in range(calc_n_lm(xEst)): plt.plot(xEst[STATE_SIZE i * 2], xEst[STATE_SIZE i * 2 1], xg) plt.plot(hxTrue[0, :], hxTrue[1, :], -b) plt.plot(hxDR[0, :], hxDR[1, :], -k) plt.plot(hxEst[0, :], hxEst[1, :], -r) plt.axis(equal) plt.grid(True) plt.pause(0.001) if __name__ __main__: main()3.1 原始代码结构# 原始实现的问题示例 def ekf_slam(xEst, PEst, u, z): # 1. 预测 S STATE_SIZE G, Fx jacob_motion(xEst[0:S], u) xEst[0:S] motion_model(xEst[0:S], u) # ❌ 错误使用Fx将过程噪声传播到所有状态 PEst[0:S, 0:S] G.T PEst[0:S, 0:S] G Fx.T Cx Fx # 2. 数据关联 for iz in range(len(z[:, 0])): min_id search_correspond_landmark_id(xEst, PEst, z[iz, 0:2]) # ❌ 错误将阈值作为候选值加入列表 min_dist.append(M_DIST_TH) min_id min_dist.index(min(min_dist)) # 3. 路标增广 initP np.eye(2) # ❌ 固定为单位矩阵 PAug np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))), np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))3.2 运行结果灾难性的发散Total Landmarks: 4 Simulation Time: 30.0s Added landmark 19 at (-11386.50, 7581.67) Added landmark 20 at (-38819.67, -1975.17) Added landmark 21 at (-37811.18, 3311.93) Added landmark 22 at (-551235.10, 1523921.69) Time: 10.0s, RMSE: 5185873.856m, Landmarks: 26/8 Time: 20.0s, RMSE: 3746475.288m, Landmarks: 26/8 Time: 30.0s, RMSE: 2469298.200m, Landmarks: 26/8 Final RMSE: 2469298.200m Landmark Detection Rate: 26/8 (325.0%)问题现象RMSE从正常的米级→数百万米路标数量从4个→26个爆炸式增长路标位置从正常→数百万米外四、调试历程6个版本的迭代优化4.1 V1.0协方差传播修复问题定位原始代码的协方差传播存在严重问题# ❌ 错误试图将过程噪声传播到所有状态 PEst[0:S, 0:S] G.T PEst[0:S, 0:S] G Fx.T Cx Fx分析Fx是(3, 32N)的矩阵Cx是(3, 3)的过程噪声相乘得到(32N, 32N)但维度不匹配修复方案# ✅ 正确只更新机器人部分的协方差 def predict(self, u): # 更新机器人状态 self.x[:STATE_SIZE] motion_model(self.x[:STATE_SIZE], u) # 更新机器人协方差 P_robot self.P[:STATE_SIZE, :STATE_SIZE] P_robot_new G P_robot G.T Q # 更新交叉协方差 P_cross self.P[:STATE_SIZE, STATE_SIZE:] P_cross_new G P_cross self.P[:STATE_SIZE, :STATE_SIZE] P_robot_new self.P[:STATE_SIZE, STATE_SIZE:] P_cross_new self.P[STATE_SIZE:, :STATE_SIZE] P_cross_new.T结果对比指标V0.0 原始V1.0 修复RMSE2,469,298m101,426m路标数26/89/4状态❌ 发散⚠️ 部分改善4.2 V2.0路标初始化优化问题定位原始代码的路标初始化initP np.eye(2) # ❌ 固定为单位矩阵 PAug np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))), np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))问题协方差固定为I不反映实际不确定性交叉协方差为零忽略机器人与路标的相关性新路标与现有路标协方差独立修复方案def add_landmark(self, measurement): # 计算路标位置 lm_pos compute_landmark_position(measurement) # 计算雅可比 H_lm jacobian_landmark_init(measurement) # ✅ 正确传播观测不确定性 P_lm H_lm P_xx H_lm.T R # ✅ 正确计算机器人-路标交叉协方差 cross_corr P_xx H_lm.T # 构建增广协方差矩阵 P_new np.zeros((new_size, new_size)) P_new[:old_size, :old_size] self.P P_new[old_size:, old_size:] P_lm P_new[:STATE_SIZE, old_size:] cross_corr P_new[old_size:, :STATE_SIZE] cross_corr.T关键知识点路标初始化的正确公式P_lm J_robot P_xx J_robot^T J_obs R J_obs^T P_x_lm P_xx J_robot^T其中J_robot路标位置对机器人状态的雅可比J_obs路标位置对观测的雅可比P_xx机器人状态协方差R观测噪声协方差4.3 V3.0数据关联重写问题定位原始数据关联的问题min_dist.append(M_DIST_TH) # ❌ 将阈值作为候选 min_id min_dist.index(min(min_dist))问题阈值被当作新路标的候选容易误判没有使用马氏距离进行有效检验关联决策过于简单修复方案def find_corresponding_landmark(self, z): min_dist float(inf) best_id None for i in range(self.N): # 计算预期观测 z_pred compute_expected_measurement(i) # 计算创新 y z - z_pred # 计算创新协方差 H compute_jacobian(i) S H P H.T R # ✅ 计算马氏距离 mahal_dist y.T inv(S) y if mahal_dist min_dist and mahal_dist MAHALANOBIS_TH: min_dist mahal_dist best_id i return best_id马氏距离 vs 欧氏距离距离类型公式优点缺点欧氏距离d sqrt(dx² dy²)计算简单忽略不确定性马氏距离d yᵀS⁻¹y考虑协方差计算复杂4.4 V4.0严格阈值与路标管理参数调优历程版本马氏阈值新路标距离观测要求最大路标结果V3.05.9913.0m1次∞路标爆炸V4.03.00.8m3次10重复创建V4.52.00.8m3次4✅ 完美最终参数配置dataclass class SLAMConfig: # 数据关联 - 严格阈值 MAHALANOBIS_TH: float 2.0 # 80%置信度 NEW_LM_DIST_TH: float 0.8 # 0.8m最小距离 MAX_LANDMARKS: int 4 # 精确匹配真实数 MIN_OBSERVATIONS: int 3 # 需要3次观测 MERGE_DIST_TH: float 0.5 # 合并阈值4.5 V5.0路标合并机制问题发现即使有了严格阈值系统仍然会创建重复路标Added landmark 0 at (10.11, -1.82) Added landmark 1 at (14.86, 10.38) Added landmark 2 at (3.48, 14.90) Added landmark 3 at (15.30, 9.67) # 与landmark 1重复解决方案自动合并def merge_landmarks(self): to_merge [] for i in range(self.N): for j in range(i 1, self.N): dist np.linalg.norm(lm_i - lm_j) if dist MERGE_DIST_TH: to_merge.append((i, j)) for i, j in to_merge: # 保留观测次数多的 if obs_count[i] obs_count[j]: keep_id, remove_id i, j else: keep_id, remove_id j, i self.remove_landmark(remove_id)路标合并的效果Merging landmark 3 into 1 (distance: 0.463m) 最终 Landmarks: 4/4 ✅4.6 V6.0数值稳定性增强Joseph形式协方差更新# ❌ 标准形式可能不稳定 P (I - KH) P # ✅ Joseph形式数值稳定 P (I - KH) P (I - KH).T K R K.T正定性保证# 确保对称 P (P P.T) / 2 # 防止负特征值 eigenvals np.linalg.eigvalsh(P) if np.min(eigenvals) 0: P np.eye(P.shape[0]) * (abs(np.min(eigenvals)) EPSILON)五、技术深度解析5.1 协方差传播详解完整的状态向量x [x_robot, y_robot, θ_robot, lm1_x, lm1_y, lm2_x, lm2_y, ...]ᵀ协方差矩阵结构P [P_xx P_xm] [P_mx P_mm]其中P_xx机器人状态协方差 (3×3)P_mm路标状态协方差 (2N×2N)P_xm机器人-路标交叉协方差 (3×2N)预测步骤的协方差传播P_xx_new G P_xx G.T V Q V.T P_xm_new G P_xm P_mm_new P_mm # 不变5.2 雅可比计算的正确姿势观测模型的雅可比观测模型z [range, bearing]ᵀdef compute_jacobian(self, lm_id): dx lm_x - robot_x dy lm_y - robot_y q dx² dy² sqrt_q sqrt(q) H np.zeros((2, STATE_SIZE 2*N)) # 对机器人状态的导数 H[0, 0] -dx / sqrt_q # ∂range/∂x H[0, 1] -dy / sqrt_q # ∂range/∂y H[1, 0] dy / q # ∂bearing/∂x H[1, 1] -dx / q # ∂bearing/∂y H[1, 2] -1 # ∂bearing/∂θ # 对路标状态的导数 H[0, lm_idx] dx / sqrt_q # ∂range/∂lm_x H[0, lm_idx1] dy / sqrt_q # ∂range/∂lm_y H[1, lm_idx] -dy / q # ∂bearing/∂lm_x H[1, lm_idx1] dx / q # ∂bearing/∂lm_y return H5.3 数据关联的最佳实践马氏距离计算def mahalanobis_distance(y, S): return float(y.T np.linalg.solve(S, y))阈值选择指南自由度90%置信度95%置信度99%置信度12.7063.8416.63524.6055.9919.21036.2517.81511.345经验法则定位精度要求高使用90%阈值4.605路标检测要求高使用95%阈值5.991系统不稳定时使用更严格阈值2.0-3.0六、性能优化技巧6.1 矩阵运算优化# ❌ 避免使用 K P H.T np.linalg.inv(S) # ✅ 使用solve K np.linalg.solve(S, H P.T).T # ✅ 备选方案数值不稳定时 K P H.T np.linalg.pinv(S)6.2 协方差正则化def regularize_covariance(P, epsilon1e-8): # 对称化 P (P P.T) / 2 # 正定化 eigenvals np.linalg.eigvalsh(P) if np.min(eigenvals) 0: P np.eye(P.shape[0]) * (abs(np.min(eigenvals)) epsilon) return P6.3 异常值检测def is_outlier(y, S, threshold7.815): try: dist float(y.T np.linalg.solve(S, y)) return dist threshold except: return True # 数值问题视为异常七、调试工具与技巧7.1 可视化调试def plot_landmark_uncertainty(lm_pos, P_lm, sigma2.0): eigenvals, eigenvecs np.linalg.eigh(P_lm) angles np.linspace(0, 2*np.pi, 30) ellipse np.array([ sigma * sqrt(max(eigenvals[0], 0)) * cos(angles), sigma * sqrt(max(eigenvals[1], 0)) * sin(angles) ]) ellipse eigenvecs ellipse plt.plot(lm_pos[0] ellipse[0, :], lm_pos[1] ellipse[1, :])7.2 日志与监控class SLAMLogger: def __init__(self): self.metrics { rmse: [], landmark_count: [], mahalanobis_dist: [], innovation_norm: [] } def log_step(self, slam, true_pos): error np.linalg.norm(true_pos - slam.mu[:2]) self.metrics[rmse].append(error) self.metrics[landmark_count].append(slam.N)7.3 单元测试pythondef test_jacobian_computation(): # 数值雅可比 vs 解析雅可比 delta 1e-6 H_analytic compute_jacobian(lm_id) # 数值计算 H_numeric np.zeros_like(H_analytic) for i in range(state_size): x_plus x.copy() x_plus[i] delta z_plus compute_measurement(x_plus) x_minus x.copy() x_minus[i] - delta z_minus compute_measurement(x_minus) H_numeric[:, i] (z_plus - z_minus) / (2 * delta) assert np.allclose(H_analytic, H_numeric, rtol1e-5)八、最终代码框架8.1 核心类结构dataclass class SLAMConfig: STATE_SIZE: int 3 LM_SIZE: int 2 Q: np.ndarray None # 过程噪声 R: np.ndarray None # 观测噪声 MAHALANOBIS_TH: float 2.0 MAX_LANDMARKS: int 4 MIN_OBSERVATIONS: int 3 class EKFSLAM: def __init__(self, config): self.mu np.zeros((STATE_SIZE, 1)) self.sigma np.eye(STATE_SIZE) * 0.01 self.N 0 def predict(self, u): ... def update(self, z): ... def find_corresponding_landmark(self, z): ... def add_landmark(self, measurement): ... def update_landmark(self, z, lm_id): ... def merge_landmarks(self): ... def remove_landmark(self, lm_id): ... def compute_jacobian(self, lm_id): ... def compute_innovation(self, z, lm_id): ...8.2 使用示例# 配置 config SLAMConfig( MAHALANOBIS_TH2.0, MAX_LANDMARKS4, MIN_OBSERVATIONS3 ) # 初始化 slam EKFSLAM(config) # 主循环 while time SIM_TIME: # 预测 slam.predict(control) # 更新 if len(observations) 0: slam.update(observations) # 评估 error np.linalg.norm(true_pos - slam.mu[:2])九、经验总结9.1 关键成功因素正确的数学实现协方差传播使用正确的雅可比路标初始化传播观测不确定性协方差更新使用Joseph形式严格的数据关联使用马氏距离考虑不确定性合适的阈值2.0-3.0异常值检测与拒绝完善的路标管理限制最大数量需要多次观测才使用自动合并相近路标数值稳定性使用solve代替inv协方差正则化角度归一化9.2 常见陷阱与解决方案陷阱症状解决方案协方差爆炸路标位置发散检查雅可比、使用Joseph形式路标无限增长Landmark count real严格阈值、限制数量、合并矩阵奇异数值错误使用solve、正则化数据关联失败同一路标多个ID降低阈值、增加观测要求角度异常航向角跳变归一化到[-π, π]9.3 调试检查清单□检查协方差矩阵是否对称正定□验证雅可比矩阵的正确性数值对比□监控马氏距离的分布□检查路标数量是否合理□验证航向角是否在[-π, π]范围内□确认噪声参数与实际情况匹配□测试不同初始条件的稳定性9.4 进一步优化方向自适应噪声根据实际误差动态调整Q和R使用协方差匹配技术鲁棒统计使用M估计器处理异常值实现Students t分布滤波器数据关联增强实现JCBB (Joint Compatibility Branch and Bound)使用ML (Maximum Likelihood) 关联性能优化使用稀疏矩阵表示实现信息滤波器形式十、参考资料10.1 经典论文Thrun, S., Burgard, W., Fox, D. (2005).Probabilistic Robotics. MIT Press.Durrant-Whyte, H., Bailey, T. (2006). Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms.IEEE Robotics Automation Magazine.Smith, R., Self, M., Cheeseman, P. (1990). Estimating Uncertain Spatial Relationships in Robotics.Autonomous Robot Vehicles.10.2 开源参考PythonRobotics - 原始代码来源GTSAM - 图优化SLAMORB-SLAM3 - 视觉SLAM10.3 相关文章从零实现EKF-SLAMSLAM中的卡尔曼滤波EKF-SLAM详解十一、结语从RMSE数百万米的灾难性发散到29.1厘米的精确估计这段调试历程让我深刻体会到数学是基础EKF-SLAM的每个公式都需要精确实现细节决定成败协方差正则化、角度归一化等小细节至关重要调试需要耐心从数据关联到路标管理每个环节都值得仔细检查系统思维很重要不能只关注单个模块要从全局角度优化希望这篇文章能帮助你在SLAM的道路上少走弯路。如果觉得有用欢迎点赞收藏最后送给所有SLAM开发者一句话The map is not the territory, but a good SLAM system can make it almost indistinguishable.附录核心参数配置dataclass class SLAMConfig: STATE_SIZE: int 3 LM_SIZE: int 2 Q: np.ndarray np.diag([0.01, 0.01]) ** 2 R: np.ndarray np.diag([0.02, np.deg2rad(1.0)]) ** 2 DT: float 0.1 SIM_TIME: float 30.0 MAX_RANGE: float 20.0 MAHALANOBIS_TH: float 2.0 NEW_LM_DIST_TH: float 0.8 MAX_LANDMARKS: int 4 MIN_OBSERVATIONS: int 3 MERGE_DIST_TH: float 0.5 EPSILON: float 1e-8运行结果Final RMSE: 0.291m (29.1cm) Final Landmarks: 4/4 ✅本文由佳木逢钺原创转载请注明出处。如有问题欢迎在评论区讨论