分类：机器人学 | 自动驾驶 | SLAM
标签：EKF-SLAM、状态估计、机器人定位、数据关联、Python

一、引言：当SLAM系统爆炸时，我在想什么？

你是否遇到过这样的场景：精心设计的EKF-SLAM算法，在仿真中RMSE飙升至数百万米，路标数量从4个爆炸到100多个，系统完全失控？

这正是我在调试一个经典EKF-SLAM实现时遇到的真实情况。本文将完整记录我从完全失败到厘米级精度的整个调试过程，包含：

• 6个版本的迭代演进

• 15+个关键Bug的定位与修复

• 10+个性能指标的对比分析

• 实用的调试技巧和经验总结

无论你是SLAM初学者还是有一定经验的开发者，本文都将帮助你避免踩坑，快速构建稳定的SLAM系统。

二、背景：什么是EKF-SLAM？

2.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, ...]

两大核心步骤：

1. 预测（Prediction）：根据运动模型更新机器人状态

2. 更新（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]], dtype=float) 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, 3+2N)的矩阵

• Cx是(3, 3)的过程噪声

• 相乘得到(3+2N, 3+2N)，但维度不匹配

修复方案

# ✅ 正确：只更新机器人部分的协方差 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

结果对比

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))))

问题：

1. 协方差固定为I，不反映实际不确定性

2. 交叉协方差为零，忽略机器人与路标的相关性

3. 新路标与现有路标协方差独立

修复方案

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))

问题：

1. 阈值被当作"新路标"的候选，容易误判

2. 没有使用马氏距离进行有效检验

3. 关联决策过于简单

修复方案

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 欧氏距离

4.4 V4.0：严格阈值与路标管理

参数调优历程

最终参数配置

@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_idx+1] = dy / sqrt_q # ∂range/∂lm_y H[1, lm_idx] = -dy / q # ∂bearing/∂lm_x H[1, lm_idx+1] = dx / q # ∂bearing/∂lm_y return H

5.3 数据关联的最佳实践

马氏距离计算：

def mahalanobis_distance(y, S): return float(y.T @ np.linalg.solve(S, y))

阈值选择指南：

经验法则：

• 定位精度要求高：使用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, epsilon=1e-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 P

6.3 异常值检测

def is_outlier(y, S, threshold=7.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, sigma=2.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 单元测试python

def 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, rtol=1e-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_TH=2.0, MAX_LANDMARKS=4, MIN_OBSERVATIONS=3 ) # 初始化 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 关键成功因素

1. 正确的数学实现

   • 协方差传播：使用正确的雅可比

   • 路标初始化：传播观测不确定性

   • 协方差更新：使用Joseph形式

2. 严格的数据关联

   • 使用马氏距离，考虑不确定性

   • 合适的阈值（2.0-3.0）

   • 异常值检测与拒绝

3. 完善的路标管理

   • 限制最大数量

   • 需要多次观测才使用

   • 自动合并相近路标

4. 数值稳定性

   • 使用solve代替inv

   • 协方差正则化

   • 角度归一化

9.2 常见陷阱与解决方案

9.3 调试检查清单

• □

  检查协方差矩阵是否对称正定

• □

  验证雅可比矩阵的正确性（数值对比）

• □

  监控马氏距离的分布

• □

  检查路标数量是否合理

• □

  验证航向角是否在[-π, π]范围内

• □

  确认噪声参数与实际情况匹配

• □

  测试不同初始条件的稳定性

9.4 进一步优化方向

1. 自适应噪声

   • 根据实际误差动态调整Q和R

   • 使用协方差匹配技术

2. 鲁棒统计

   • 使用M估计器处理异常值

   • 实现Student's t分布滤波器

3. 数据关联增强

   • 实现JCBB (Joint Compatibility Branch and Bound)

   • 使用ML (Maximum Likelihood) 关联

4. 性能优化

   • 使用稀疏矩阵表示

   • 实现信息滤波器形式

十、参考资料

10.1 经典论文

1. Thrun, S., Burgard, W., & Fox, D. (2005).Probabilistic Robotics. MIT Press.

2. Durrant-Whyte, H., & Bailey, T. (2006). "Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms".IEEE Robotics & Automation Magazine.

3. Smith, R., Self, M., & Cheeseman, P. (1990). "Estimating Uncertain Spatial Relationships in Robotics".Autonomous Robot Vehicles.

10.2 开源参考

• PythonRobotics - 原始代码来源

• GTSAM - 图优化SLAM

• ORB-SLAM3 - 视觉SLAM

10.3 相关文章

• 从零实现EKF-SLAM

• SLAM中的卡尔曼滤波

• EKF-SLAM详解

十一、结语

从RMSE数百万米的灾难性发散，到29.1厘米的精确估计，这段调试历程让我深刻体会到：

1. 数学是基础：EKF-SLAM的每个公式都需要精确实现

2. 细节决定成败：协方差正则化、角度归一化等小细节至关重要

3. 调试需要耐心：从数据关联到路标管理，每个环节都值得仔细检查

4. 系统思维很重要：不能只关注单个模块，要从全局角度优化

希望这篇文章能帮助你在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 ✅

本文由佳木逢钺原创，转载请注明出处。如有问题，欢迎在评论区讨论！