rm_vision 的ekf解算

### 关于 RM_Vision 中扩展卡尔曼滤波器（EKF）的实现 在机器人视觉处理库 `rm_vision` 中，扩展卡尔曼滤波器（Extended Kalman Filter, EKF）被广泛应用于状态估计问题。EKF 是一种用于非线性系统的递推算法，在预测和更新阶段通过泰勒展开近似处理非线性函数。 对于自由飞行机器人的姿态估计，可以采用基于关节体模型的方法来构建观测方程和运动学模型[^1]。具体到 rm_vision 的应用场景下： #### 运动模型定义 假设存在一个多刚体结构组成的机械臂或无人机系统，其动力学特性由一组微分方程描述。为了简化计算复杂度并提高实时性能，通常会忽略高阶项而保留一阶导数作为主要贡献因素。因此，离散时间下的状态转移矩阵 \( F_k \) 和过程噪声协方差矩阵 \( Q_k \) 可以表示如下： \[ x_{k} = f(x_{k-1}, u_k) + w_k \] 其中 \( x_k \in R^n \) 表示 k 时刻的状态向量；\( u_k \in R^m \) 控制输入；\( w_k ∼ N(0,Q_k)\) 零均值白噪声序列。 #### 测量模型建立 考虑到传感器数据可能存在偏差以及外界干扰的影响，测量模型同样需要考虑不确定性。设 z_k ∈R^p 为第 k 步获得的实际观测值，则有： \[ z_k=h(x_k)+v_k \] 这里 h() 函数映射真实世界中的物理位置至图像坐标系内某一点的位置关系；\( v_k∼N(0,R_k)\) 则代表了观测量误差分布情况。 #### Python 实现代码片段 下面给出一段简单的 python 伪代码用来展示如何利用 numpy 库完成一次完整的 ekf 循环迭代: ```python import numpy as np def predict_step(F, P_prev, Q): """Predict next state based on current estimate.""" # Predicted state covariance update P_pred = F @ P_prev @ F.T + Q return P_pred def measurement_update(H, y_meas, S_inv, K_gain, x_est, P_est): """Update estimates using new measurements""" innov = y_meas - H@x_est # Innovation / residual calculation S = H @ P_est @ H.T + R # Measurement prediction uncertainty K_gain = P_est @ H.T @ S_inv # Optimal kalman gain computation x_upd = x_est + K_gain @ innov # Updated state estimation P_upd = (np.eye(len(P_est)) - K_gain @ H) @ P_est # Posterior error covarience return x_upd, P_upd # Initialize parameters... F = ... # State transition matrix from previous time step to now. P = ... # Initial estimated error covariance of the system states. Q = ... # Process noise covariance describing uncertainties during transitions between steps. H = ... # Observation model mapping true position into sensor readings. R = ... # Covariance associated with observation errors. for t in range(T): # Loop over all available timestamps ... # Prediction phase P = predict_step(F[t], P, Q[t]) # Update phase when there is an actual measurement at this timestamp if not np.isnan(y_measured[t]).any(): S_inv = np.linalg.inv(S) x_hat, P = measurement_update(H[t], y_measured[t], S_inv, K_gain, x_hat, P) ```