平方根容积卡尔曼滤波流程

### 平方根容积卡尔曼滤波(SRCKF)实现流程

#### 工作原理概述
平方根容积卡尔曼滤波（Square Root Cubature Kalman Filter, SRCKF）通过引入平方根形式来提高数值稳定性并减少计算复杂度。该算法的核心在于使用球面径向 cubature 规则近似非线性函数的积分，从而获得更精确的状态估计。

#### 初始化阶段
初始化过程中设定初始状态向量 \( \hat{x}_0 \)，以及其对应的协方差矩阵 P 的 Cholesky 分解 L_0 ，即：

\[P_0 = LL^T\]

其中 \(L\) 是下三角阵[^1]。

#### 预测步骤
预测步主要完成对未来时刻系统状态及其不确定性的预报:

1. **生成sigma点集**: 计算一组加权样本点 (也称为 sigma 点), 这些点围绕当前状态分布，并携带关于均值和协方差的信息。

2. **传播sigma点**: 将这些sigma点传递给系统的动态模型 f(·)，得到新的预测位置。

3. **更新预测统计量**: 对经过变换后的sigma点求平均值得到预测状态；同时重新构建协方差矩阵以反映新状态下不确定性变化情况。

def predict(L_k_minus_1, F_k):
    # Generate Sigma Points based on L_k_minus_1
    xi_set = generate_sigma_points(L_k_minus_1)

    # Propagate through nonlinear function
    propagated_xi = [f(xi) for xi in xi_set]

    # Compute predicted mean and covariance from transformed points
    x_pred = sum(propagated_xi)/len(propagated_xi)
    
    # Update the square root of prediction error covariance matrix
    L_pred = update_square_root_covariance(propagated_xi)
    
    return x_pred, L_pred

#### 更新步骤
当接收到测量数据 z 后执行校正操作:

1. **交叉关联**：评估观测与预测之间的关系，形成互协方差 S 和增益 K。

2. **残差分析**：根据实际观察值减去预期观测量 H(x̂) 来定义残差 y。

3. **修正状态估计**：调整先前预测的结果，使之更好地匹配最新获取的数据。

4. **维护正定性质**：确保协方差矩阵保持半正定特性，这有助于维持后续迭代中的稳定性和可靠性。

def correct(z_measured, H_func, R_matrix, x_pred, L_pred):
    # Calculate cross-covariance between state predictions and measurements
    P_zz = compute_cross_covariance(H_func, L_pred)

    # Innovation or measurement residual
    innovation = z_measured - H_func(x_pred)

    # Measurement noise covariance plus process noise contribution
    S = P_zz + R_matrix
    
    # Kalman gain calculation using QR decomposition to maintain numerical stability
    Q, R_gain = qr_factorization(P_zz.T @ inv(R_matrix))
    K = triu_solve(R_gain.T, Q.T @ L_pred.T).T 

    # Corrected estimate
    x_updated = x_pred + K @ innovation

    # Square-root form of updated covariance
    L_updated = chol_update(L_pred, K @ sqrtm(S))

    return x_updated, L_updated