ESKF二维组合导航

### ESKF在二维组合导航中的应用

#### 定义与背景
ESKF（Error State Kalman Filter），是一种改进版本的卡尔曼滤波器，专门用于处理状态估计问题，特别是在存在非线性模型的情况下。该方法通过引入误差状态来近似原始系统的动态特性，从而提高了计算效率并增强了数值稳定性[^1]。

#### 实现过程
为了实现ESKF于二维集成导航系统中，需考虑如下几个方面：

- **初始化**
初始化阶段涉及设定初始位置、速度以及姿态角等参数，并定义相应的协方差矩阵P0描述这些量之间的不确定性程度。

- **预测更新**
利用运动学方程预测下一时刻的状态向量x̂k|k−1及其对应的协方差Pk|k−1。对于二维情况下的移动机器人而言，通常采用常速或匀加速直线运动假设作为动力学模型的一部分；而对于旋转部分，则可以利用四元数表示法简化计算复杂度[^4]。

- **测量更新**
当接收到新的观测数据z_k时，基于当前预测结果调整估计值。这里的关键在于构建合适的观测量雅可比H和噪声统计特征R，以便准确反映传感器读数与真实世界之间关系的同时抑制外界干扰因素的影响。

- **误差校正**
计算残差ν=zk-H(x̂k|k−1)，并通过求解最小二乘优化问题获得最优增益K=(HPHT+R)^(-1) HP，进而完成对原估计序列的修正操作：Δx̃=-K ν, x̂k=x̂k|k−1+Δx̃。

import numpy as np

def eskf_predict_update(state_mean_prev, P_prev, F, Q):
    """
    预测更新步骤
    
    参数:
        state_mean_prev : 上一时刻的状态均值
        P_prev          : 上一时刻的协方差矩阵
        F               : 状态转移矩阵
        Q               : 过程噪声协方差
        
    返回:
        pred_state      : 新一步预测后的状态均值
        pred_covar      : 新一步预测后的协方差矩阵
    """
    
    # Predicted mean and covariance of the next step's state distribution.
    pred_state = F @ state_mean_prev
    pred_covar_sqrt = scipy.linalg.sqrtm(F @ P_prev @ F.T + Q)

    return pred_state, pred_covar_sqrt


def eskf_measurement_update(pred_state, H, R, z_measured, pred_covar_sqrt):
    """
    测量更新步骤
    
    参数:
        pred_state     : 预测得到的新步状态均值
        H              : 观察函数Jacobian矩阵
        R              : 观测噪声协方差
        z_measured     : 实际观察到的数据
        pred_covar_sqrt: 预测后协方差平方根矩阵
        
    返回:
        updated_state   : 更新后的状态估计
        updated_covar   : 更新后的协方差矩阵
    """

    S = H @ pred_covar_sqrt @ H.T + R
    K = pred_covar_sqrt @ H.T @ np.linalg.inv(S)
    
    residual = z_measured - H @ pred_state
    delta_x = K @ residual
    
    updated_state = pred_state + delta_x
    updated_covar_sqrt = scipy.linalg.sqrtm((np.eye(len(K)) - K@H) @ pred_covar_sqrt)

    return updated_state, updated_covar_sqrt