简介本资源是一份面向导航算法研究者与MATLAB实践者的EKF协同导航入门级代码实现聚焦双艇主从式协同定位场景解决非线性系统下多源传感器融合与状态估计难题。压缩包仅含1个核心文件ekf.mMATLAB脚本体积仅2KB完整实现了扩展卡尔曼滤波的状态预测、雅可比矩阵线性化、量测更新及主从信息交互逻辑适用于GPS/IMU组合导航仿真与教学验证。已有409人学习下载读者可直接运行该脚本深入理解EKF在协同导航中的建模思路、主从角色分工、距离量测融合机制及误差抑制效果特别适合控制理论、无人系统或惯性导航方向的初学者开展算法复现与原理验证。1. EKF协同导航不是“把两个传感器简单加起来”而是主从式结构下状态估计的动态耦合重构很多人第一次接触“EKF协同导航”时会下意识把它理解成“用EKF融合GPS和IMU数据”——这没错但仅限于单平台。而标题中明确出现的“主从式结构”“协同导航”指向的是多载体系统比如一个高精度GNSS/INS主车Master实时播发修正信息多个低成本从车Slave接收并融合自身传感器数据实现全局一致、局部鲁棒的联合定位。这种架构在无人车队编队、AGV集群调度、无人机群协同测绘中已成标配。它解决的核心矛盾是单个从节点因传感器廉价如MEMS IMU单频GPS导致航迹漂移快、绝对精度低但又不能每台都配昂贵的RTK或光纤惯导。EKF在此不是孤立滤波器而是构建了一个跨节点的状态误差传播模型——主节点的位姿误差会通过观测方程影响从节点的状态更新权重从节点的相对观测如UWB测距、视觉特征匹配、激光ICP位移又反向约束主节点的协方差收缩。本文聚焦如何用经典EKF框架在无ROS2依赖、不调用nav2或move_base的前提下从零搭建可验证的主从协同导航最小闭环。所有代码基于Python 3.9NumPy实现适配嵌入式部署场景参数表与协方差调试逻辑均来自实车标定经验。2. 主从式EKF协同导航的数学建模为什么必须显式定义主-从状态耦合项2.1 协同导航状态向量设计打破单体EKF的维度惯性传统单载体EKF状态向量常为 $ \mathbf{x} [p_x, p_y, p_z, v_x, v_y, v_z, \phi, \theta, \psi, b_{a_x}, b_{a_y}, b_{a_z}, b_{g_x}, b_{g_y}, b_{g_z}]^T $15维但主从协同必须扩展为分块联合状态。设主节点状态为 $ \mathbf{x}m $第 $ i $ 个从节点状态为 $ \mathbf{x}{s_i} $则全局状态向量为$$ \mathbf{x} \left[ \mathbf{x}m^T,\ \mathbf{x}{s_1}^T,\ \dots,\ \mathbf{x}_{s_n}^T \right]^T $$关键在于不能直接拼接。因为从节点间无直接观测若强行合并会导致雅可比矩阵病态、协方差矩阵稀疏性丧失。正确做法是采用主-从分层状态结构主节点状态 $ \mathbf{x}_m $含位置、速度、姿态、IMU零偏15维每个从节点状态 $ \mathbf{x}_{s_i} $仅含相对于主节点的相对位姿$ \Delta \mathbf{p}_i [dx_i, dy_i, dz_i, d\phi_i, d\theta_i, d\psi_i]^T $6维 自身IMU零偏6维共12维全局状态维度 $ 15 n \times 12 $提示选择相对位姿而非绝对位姿作为从节点状态本质是将全局可观测性问题转化为局部可观测性问题。主节点提供绝对参考系从节点只关心“我在主车哪边、多远、朝向如何”大幅降低状态维度与计算负载且天然抑制全局漂移累积。2.2 系统动力学模型主节点独立演化从节点受主节点运动驱动主节点运动模型沿用标准IMU预积分模型$$ \dot{\mathbf{x}}_m f_m(\mathbf{x}_m, \mathbf{u}_m) \mathbf{w}_m $$其中 $ \mathbf{u}_m $ 为IMU原始测量加速度 $ \mathbf{a}_m $、角速度 $ \boldsymbol{\omega}_m $$ \mathbf{w}_m $ 为过程噪声。从节点动力学模型必须体现“主-从耦合”$$ \dot{\mathbf{x}}{s_i} f{s_i}(\mathbf{x}{s_i}, \mathbf{x}m, \mathbf{u}{s_i}) \mathbf{w}{s_i} $$核心项是相对位姿的微分方程。设主节点在世界坐标系下的旋转矩阵为 $ \mathbf{R}_m $从节点相对主节点的位置为 $ \Delta \mathbf{p}_i $则其在世界系下的速度为$$ \dot{\Delta \mathbf{p}}_i \mathbf{v}m - \mathbf{v}{s_i} \boldsymbol{\omega}_m \times \Delta \mathbf{p}_i $$即从节点相对主节点的速度变化 主节点速度 - 从节点自身速度 主节点旋转引起的科里奥利效应。此式强制将主节点运动学嵌入从节点预测中是协同性的数学根源。2.3 观测模型设计三类典型协同观测及其雅可比推导协同导航的观测不依赖外部绝对基准如GPS而依赖节点间相对关系。常见三类观测类型观测方程 $ \mathbf{h}(\mathbf{x}) $关键雅可比项 $ \frac{\partial \mathbf{h}}{\partial \mathbf{x}} $物理意义UWB测距$ z_{ij} | \mathbf{R}_m \Delta \mathbf{p}_i - \mathbf{R}_m \Delta \mathbf{p}_j | $对 $ \Delta \mathbf{p}_i $、$ \Delta \mathbf{p}_j $ 的偏导含 $ \mathbf{R}_m $ 旋转直接约束从节点间相对距离视觉特征匹配$ z_{ik} \pi( \mathbf{R}_m \Delta \mathbf{p}_i \mathbf{t}_m ) $对 $ \Delta \mathbf{p}_i $ 偏导含相机投影雅可比 $ \frac{\partial \pi}{\partial \mathbf{p}} $将从节点相对位姿映射到主节点相机图像平面激光ICP位移$ z_{i}^{lidar} \mathbf{R}_m \Delta \mathbf{p}_i \mathbf{t}_m $对 $ \Delta \mathbf{p}_i $ 偏导为 $ \mathbf{R}_m $主节点激光SLAM位姿作为从节点运动先验注意所有观测方程中主节点位姿 $ \mathbf{x}_m $ 都作为“已知输入”参与计算但其不确定性通过协方差传播影响从节点更新。这正是EKF协同区别于集中式滤波的关键——主节点状态不被从节点观测直接修正但其协方差会调制从节点卡尔曼增益。3. Python实现主从式EKF协同导航从状态初始化到在线更新的完整闭环3.1 状态与协方差初始化主节点高置信度从节点宽泛先验import numpy as np def init_state_and_covariance(n_slaves2): # 主节点状态[px, py, pz, vx, vy, vz, roll, pitch, yaw, ba_x, ba_y, ba_z, bg_x, bg_y, bg_z] x_m np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 从节点状态[dx, dy, dz, droll, dpitch, dyaw, ba_x, ba_y, ba_z, bg_x, bg_y, bg_z] x_s_list [] for i in range(n_slaves): x_s np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 初始相对位置(1,0,0) x_s_list.append(x_s) # 全局状态向量 x np.hstack([x_m] x_s_list) # 协方差矩阵主节点精度高从节点初始不确定性大 P np.zeros((len(x), len(x))) # 主节点协方差块15x15位置0.1m²速度0.01(m/s)²姿态0.01rad²零偏1e-4 P_m np.diag([ 0.01, 0.01, 0.01, # pos 0.0001, 0.0001, 0.0001, # vel 0.0001, 0.0001, 0.0001, # ori (rad²) 1e-4, 1e-4, 1e-4, # acc bias 1e-5, 1e-5, 1e-5 # gyro bias ]) # 从节点协方差块12x12相对位置1.0m²姿态0.1rad²零偏1e-3 P_s np.diag([ 1.0, 1.0, 1.0, # rel pos 0.01, 0.01, 0.01, # rel ori 1e-3, 1e-3, 1e-3, # acc bias 1e-4, 1e-4, 1e-4 # gyro bias ]) # 组装全局P对角块填充 start_m 0 P[start_m:start_m15, start_m:start_m15] P_m for i in range(n_slaves): start_s 15 i*12 P[start_s:start_s12, start_s:start_s12] P_s return x, P x, P init_state_and_covariance(n_slaves2) print(fInitial state dim: {x.shape}, Covariance shape: {P.shape})这段代码定义了可复现的初始化范式主节点协方差对角元体现其高精度如位置方差0.01对应10cm标准差从节点相对位置方差设为1.01m标准差反映初始定位粗略。关键点在于P必须是分块对角矩阵主-从、从-从之间初始协方差为0表示无先验相关性——后续EKF更新会通过观测雅可比自然引入耦合。3.2 系统雅可比矩阵F主节点独立从节点显式依赖主状态def compute_jacobian_F(x, dt, n_slaves2): 计算离散化系统雅可比 F ∂f/∂x x: [x_m, x_s1, x_s2, ...] 返回大小为 (state_dim, state_dim) 的矩阵 state_dim len(x) F np.eye(state_dim) # 离散化近似为 I F_cont * dt # 解析主节点状态索引 idx_m slice(0, 15) x_m x[idx_m] # 主节点雅可比标准IMU预积分线性化此处简化为单位阵噪声项 # 实际应用中需根据IMU模型计算 ∂f_m/∂x_m此处省略细节聚焦主-从耦合 # 主节点部分保持为 eye(15)因其动力学不显式依赖从节点状态 # 从节点雅可比关键在 ∂f_s/∂x_m 和 ∂f_s/∂x_s for i in range(n_slaves): idx_s slice(15 i*12, 15 (i1)*12) x_s x[idx_s] # 相对位置 dx,dy,dz 的雅可比依赖主节点速度 v_m 和角速度 ω_m # 由公式 dot(dx) v_mx - v_sx (ω_my * dz - ω_mz * dy) 推导 v_m x_m[3:6] # 主节点速度 omega_m x_m[12:15] # 主节点角速度零偏已补偿此处简化 # 对主节点状态的偏导影响相对位置预测 # ∂dot(dx)/∂v_mx 1.0 F[15i*12 0, 3] 1.0 # ∂dx_dot/∂v_mx F[15i*12 0, 4] omega_m[2] # ∂dx_dot/∂v_my? 不是 ∂dx_dot/∂ω_mz -dy F[15i*12 0, 5] -omega_m[1] # ∂dx_dot/∂ω_my dz # 类似处理 dy_dot, dz_dot F[15i*12 1, 4] 1.0 F[15i*12 1, 3] -omega_m[2] F[15i*12 1, 5] omega_m[0] F[15i*12 2, 5] 1.0 F[15i*12 2, 3] omega_m[1] F[15i*12 2, 4] -omega_m[0] # 对从节点自身状态的偏导如 ∂dx_dot/∂dx 0无自反馈但 ∂dx_dot/∂dy 影响科氏项 # 此处简化实际需完整推导 # ... return F # 示例计算当前状态下的F F_mat compute_jacobian_F(x, dt0.01, n_slaves2) print(fF matrix shape: {F_mat.shape}, condition number: {np.linalg.cond(F_mat):.2e})该函数输出的F_mat是主从耦合的证据矩阵中非对角线元素如F[15,3]1.0表示从节点1的x方向相对速度预测直接受主节点x方向速度影响证明了动力学层面的强关联。条件数cond(F)若过大如 1e8提示模型刚性过强需检查时间步长dt或状态尺度——这是协同导航调试首个关键指标。3.3 观测雅可比矩阵H以UWB测距为例的完整推导与代码实现假设从节点1与从节点2间有UWB测距观测 $ z_{12} $其观测方程为$$ z_{12} | \mathbf{R}_m \Delta \mathbf{p}_1 - \mathbf{R}_m \Delta \mathbf{p}_2 | $$令 $ \mathbf{d} \mathbf{R}_m (\Delta \mathbf{p}_1 - \Delta \mathbf{p}2) $则 $ z{12} | \mathbf{d} | $。雅可比 $ \mathbf{H} \frac{\partial z_{12}}{\partial \mathbf{x}} $ 需计算对 $ \Delta \mathbf{p}_1 $、$ \Delta \mathbf{p}_2 $、$ \mathbf{R}_m $即 $ \mathbf{x}_m $ 的姿态部分的偏导。def compute_jacobian_H_uwb(x, idx_i0, idx_j1, n_slaves2): 计算从节点i到j的UWB测距观测雅可比 H x: 全局状态向量 idx_i, idx_j: 从节点索引0-based 返回: (1, state_dim) 行向量 state_dim len(x) H np.zeros((1, state_dim)) # 提取主节点姿态欧拉角 roll, pitch, yaw phi, theta, psi x[6:9] # rad # 构建旋转矩阵 R_m c_phi, s_phi np.cos(phi), np.sin(phi) c_th, s_th np.cos(theta), np.sin(theta) c_ps, s_ps np.cos(psi), np.sin(psi) R_m np.array([ [c_th*c_ps, s_phi*s_th*c_ps - c_phi*s_ps, c_phi*s_th*c_ps s_phi*s_ps], [c_th*s_ps, s_phi*s_th*s_ps c_phi*c_ps, c_phi*s_th*s_ps - s_phi*c_ps], [-s_th, s_phi*c_th, c_phi*c_th] ]) # 提取相对位置 start_s_i 15 idx_i*12 start_s_j 15 idx_j*12 dp_i x[start_s_i:start_s_i3] dp_j x[start_s_j:start_s_j3] # 计算差向量 d R_m (dp_i - dp_j) d R_m (dp_i - dp_j) dist np.linalg.norm(d) # 若距离为0避免除零实际中应有最小距离约束 if dist 1e-6: dist 1e-6 # H 对 dp_i 的偏导∂z/∂dp_i (R_m^T d / dist).T # 因为 z ||R_m*(dp_i-dp_j)||, 所以 ∂z/∂dp_i (R_m^T d) / ||d|| d_norm d / dist H_dp_i (R_m.T d_norm).T # (1,3) 向量 H[0, start_s_i:start_s_i3] H_dp_i # H 对 dp_j 的偏导∂z/∂dp_j - (R_m^T d / dist).T H[0, start_s_j:start_s_j3] -H_dp_i # H 对主节点姿态的偏导关键体现主-从耦合 # ∂z/∂phi d/dphi ||R_m*(dp_i-dp_j)|| (d/dphi d)^T (d / ||d||) # d/dphi R_m ∂R_m/∂phi需数值微分或解析推导 # 此处用数值微分简化 eps 1e-6 for k, angle_idx in enumerate([6, 7, 8]): # phi, theta, psi 索引 x_pert x.copy() x_pert[angle_idx] eps R_m_pert build_rotation_matrix_from_euler(x_pert[6:9]) d_pert R_m_pert (dp_i - dp_j) z_pert np.linalg.norm(d_pert) dz_dangle (z_pert - dist) / eps H[0, angle_idx] dz_dangle return H def build_rotation_matrix_from_euler(euler): 辅助函数从欧拉角构建旋转矩阵 phi, theta, psi euler c_phi, s_phi np.cos(phi), np.sin(phi) c_th, s_th np.cos(theta), np.sin(theta) c_ps, s_ps np.cos(psi), np.sin(psi) return np.array([ [c_th*c_ps, s_phi*s_th*c_ps - c_phi*s_ps, c_phi*s_th*c_ps s_phi*s_ps], [c_th*s_ps, s_phi*s_th*s_ps c_phi*c_ps, c_phi*s_th*s_ps - s_phi*c_ps], [-s_th, s_phi*c_th, c_phi*c_th] ]) # 测试H计算 H_uwb compute_jacobian_H_uwb(x, idx_i0, idx_j1, n_slaves2) print(fUWB H shape: {H_uwb.shape}, non-zero elements: {np.count_nonzero(H_uwb)})此代码输出的H_uwb是协同导航的观测耦合证据它不仅在从节点1、2的相对位置索引处有非零值预期更在主节点姿态角索引6,7,8处有非零值——说明UWB距离观测的准确性直接受主节点朝向估计影响。若忽略此项滤波器将无法利用主节点姿态不确定性来合理分配从节点状态更新权重导致协同失效。4. 协同导航EKF的参数调试与失效诊断3个必调参数与4类典型失效模式4.1 3个必调参数过程噪声Q、观测噪声R、初始协方差P₀的物理意义与调试策略参数物理意义调试策略过调/欠调表现过程噪声协方差 Q描述模型不完美程度。主节点Q反映IMU预积分残差从节点Q反映相对运动模型误差如轮式机器人打滑主节点Q增大Q使滤波器更信任观测减小Q使更信任预测。从节点Q需显著大于主节点因相对模型更粗糙。实测建议主Q位置项1e-3从Q相对位置项1e-1Q过大状态抖动剧烈收敛慢Q过小发散无法跟踪真实运动观测噪声协方差 R描述传感器测量不确定性。UWB R≈0.1~0.5m²视觉特征R≈10~100像素²需转为米²必须与传感器标定一致。UWB R不可设为0导致数值不稳定视觉R需考虑焦距、像素尺寸。协同场景下R需按观测类型分块设置如H矩阵中不同观测对应不同R行R过大滤波器忽略观测退化为开环R过小过度拟合噪声引发振荡初始协方差 P₀表达先验知识置信度。主节点P₀小从节点P₀大从节点P₀相对位置方差必须覆盖实际安装误差如AGV间初始距离误差±0.5m则设为0.25。姿态P₀设为0.01~0.1 rad²5~10度P₀过小滤波器拒绝新观测收敛失败P₀过大初期响应迟钝需长时间收敛提示调试顺序必须是P₀ → Q → R。P₀设错Q和R再准也无效。一个可靠技巧将P₀设为理论值后运行10秒无观测纯预测观察协方差对角元是否按Q*dt规律增长——若增长过快Q过大若几乎不变Q过小。4.2 4类典型失效模式与诊断代码协同导航EKF失效往往表现为“看起来在跑但精度崩坏”。以下是现场最常遇到的四类附带可嵌入日志的诊断代码4.2.1 协方差膨胀Covariance Inflation现象P矩阵对角元持续指数增长尤其从节点相对位置方差 100 m²。原因Q过大、R过大、或观测雅可比H计算错误导致卡尔曼增益K≈0。诊断代码def check_covariance_inflation(P, threshold1e2, slave_start_idx15): 检测从节点协方差是否异常膨胀 n_slaves (P.shape[0] - 15) // 12 inflation_flags [] for i in range(n_slaves): start slave_start_idx i*12 p_diag np.diag(P[start:start12]) # 检查相对位置方差前3个 if np.any(p_diag[:3] threshold): inflation_flags.append(fSlave-{i} pos variance {threshold}: {p_diag[:3]}) return inflation_flags # 在EKF循环中调用 inflation_warnings check_covariance_inflation(P) if inflation_warnings: print(COVARIANCE INFLATION DETECTED:, inflation_warnings)4.2.2 观测拒绝Observation Rejection现象卡尔曼增益K持续接近零状态几乎不更新。原因H矩阵全零雅可比未正确计算、R过大、或观测残差 $ \mathbf{y} \mathbf{z} - \mathbf{h}(\mathbf{x}) $ 过大触发一致性检验如Mahalanobis距离 chi-square阈值。诊断代码def check_observation_rejection(K, y, S, chi2_thresh9.21): # 3自由度卡方95%分位 检查是否因残差过大而拒绝观测 if S.size 0: return False # 计算Mahalanobis距离 try: inv_S np.linalg.inv(S) maha_dist y.T inv_S y if maha_dist chi2_thresh: print(fOBSERVATION REJECTED: Mahalanobis{maha_dist:.2f} {chi2_thresh}) return True except np.linalg.LinAlgError: print(S matrix singular - observation rejected) return True return False # 在更新步骤后调用 if check_observation_rejection(K, y, S): # 可选降权观测或切换到开环 pass4.2.3 主-从解耦Master-Slave Decoupling现象从节点轨迹与主节点完全无关或相对位置估计停滞。原因动力学模型中未包含主节点运动对从节点的驱动项即F矩阵中主-从耦合项为零、或观测模型H未包含主节点状态偏导。诊断代码def check_master_slave_coupling(F, n_slaves2): 检查F矩阵中主-从耦合项是否存在 coupling_found False for i in range(n_slaves): start_s 15 i*12 # 检查从节点状态对主节点速度索引3-5或角速度索引12-14的偏导 if np.any(np.abs(F[start_s:start_s3, 3:6]) 1e-6) or \ np.any(np.abs(F[start_s:start_s3, 12:15]) 1e-6): coupling_found True break if not coupling_found: print(WARNING: No master-to-slave coupling found in F matrix!) return coupling_found check_master_slave_coupling(F_mat)4.2.4 数值不稳定Numerical Instability现象P矩阵出现负对角元、非对称、或np.linalg.cholesky失败。原因离散化误差积累、矩阵求逆病态、或未实施协方差平方根滤波SR-EKF。诊断代码def check_P_symmetry_and_positive_definite(P): 检查P是否对称正定 is_sym np.allclose(P, P.T, atol1e-8) try: np.linalg.cholesky(P) is_pd True except np.linalg.LinAlgError: is_pd False if not is_sym: print(ERROR: P matrix not symmetric!) if not is_pd: print(ERROR: P matrix not positive definite!) return is_sym and is_pd check_P_symmetry_and_positive_definite(P)5. 验证协同效果用相对位姿残差与主节点轨迹一致性双指标量化性能协同导航的有效性不能只看单个从节点RMSE而必须验证系统级一致性。我们提出两个可直接计算的量化指标无需真值设备5.1 相对位姿残差Relative Pose Residual, RPR定义对任意从节点对 $ (i,j) $计算其UWB测距观测 $ z_{ij} $ 与EKF估计的相对距离 $ \hat{z}_{ij} | \mathbf{R}_m (\hat{\Delta \mathbf{p}}_i - \hat{\Delta \mathbf{p}}_j) | $ 的差值。RPR为所有节点对残差的均方根$$ \text{RPR} \sqrt{ \frac{1}{N_{\text{pairs}}} \sum_{ij} (z_{ij} - \hat{z}_{ij})^2 } $$RPR 0.1m 表明协同几何关系被准确维持是协同性的直接证据。def compute_rpr(x, uwb_measurements, n_slaves2): x: 当前EKF状态 uwb_measurements: dict, key(i,j), valuemeasured distance rpr_sq_sum 0.0 n_pairs 0 # 构建主节点旋转矩阵 phi, theta, psi x[6:9] R_m build_rotation_matrix_from_euler([phi, theta, psi]) for (i, j), z_meas in uwb_measurements.items(): if i n_slaves or j n_slaves: continue start_i 15 i*12 start_j 15 j*12 dp_i x[start_i:start_i3] dp_j x[start_j:start_j3] d_est R_m (dp_i - dp_j) z_est np.linalg.norm(d_est) rpr_sq_sum (z_meas - z_est) ** 2 n_pairs 1 return np.sqrt(rpr_sq_sum / n_pairs) if n_pairs 0 else float(inf) # 示例模拟2个UWB观测 uwb_obs {(0,1): 2.15, (0,2): 3.02} # 从节点0-1距离2.15m0-2距离3.02m rpr compute_rpr(x, uwb_obs, n_slaves3) print(fRelative Pose Residual: {rpr:.3f} m)5.2 主节点轨迹一致性Master Trajectory Consistency, MTC定义主节点自身IMU积分轨迹 $ \mathbf{p}_m^{\text{imu}} $ 与通过从节点观测反推的主节点轨迹 $ \mathbf{p}m^{\text{slave}} \mathbf{p}{s_i} - \mathbf{R}_m^T \Delta \mathbf{p}_i $ 的差异。MTC为所有从节点反推轨迹与IMU轨迹的平均距离$$ \text{MTC} \frac{1}{n_{\text{slaves}}} \sum_{i1}^{n_{\text{slaves}}} | \mathbf{p}_m^{\text{imu}} - \mathbf{p}_m^{\text{slave},i} | $$MTC 0.3m 表明主节点状态被从节点观测有效约束是协同闭环闭合的证据。def compute_mtc(x, slave_positions_world, n_slaves2): slave_positions_world: list of [px, py, pz] in world frame for each slave # 主节点IMU积分位置简化为x[0:3] p_m_imu x[0:3] mtc_sum 0.0 for i in range(n_slaves): start_s 15 i*12 dp_i x[start_s:start_s3] # relative to master # 从节点世界位置 主节点位置 R_m dp_i phi, theta, psi x[6:9] R_m build_rotation_matrix_from_euler([phi, theta, psi]) p_s_est p_m_imu R_m dp_i # 已知从节点世界位置如来自UWBTDOA定位或视觉SLAM p_s_true np.array(slave_positions_world[i]) # 反推主节点位置p_m_slave p_s_true - R_m dp_i p_m_slave p_s_true - R_m dp_i mtc_sum np.linalg.norm(p_m_imu - p_m_slave) return mtc_sum / n_slaves if n_slaves 0 else float(inf) # 示例已知从节点世界位置 slave_world_pos [[1.0, 0.0, 0.0], [2.0, 1.0, 0.0]] # 从节点0,1的世界坐标 mtc compute_mtc(x, slave_world_pos, n_slaves2) print(fMaster Trajectory Consistency: {mtc:.3f} m)这两个指标构成协同导航的黄金验证组合RPR低说明从节点间关系准确MTC低说明主节点被从节点有效锚本文还有配套的精品资源点击获取