平方根容积卡尔曼滤波的公式

平方根容积卡尔曼滤波（Square Root Cubature Kalman Filter，SRCKF）的公式较为复杂，以下是其中的一部分：

预测步骤：

$$\hat{x}_{k+1|k} = f(\hat{x}_{k|k},u_k)+B_ku_k$$

$$\hat{y}_{k+1|k} = h(\hat{x}_{k+1|k})$$

$$F_{k+1} = \frac{\partial f}{\partial x}|_{\hat{x}_{k|k},u_k}$$

$$Q_{k+1} = G_kQ_kG_k^T+R_k$$

$$P_{k+1|k} = F_{k+1}P_{k|k}F_{k+1}^T+Q_{k+1}$$

其中，$\hat{x}_{k|k}$表示时刻$k$的状态估计值，$u_k$表示$k$时刻的输入，$f$和$h$为非线性函数，$B_k$为输入矩阵，$F_{k+1}$为状态转移矩阵，$Q_{k+1}$为过程噪声协方差矩阵，$G_k$为噪声影响矩阵，$R_k$为测量噪声协方差矩阵，$P_{k|k}$为状态估计协方差矩阵。

更新步骤：

$$K_{k+1} = P_{k+1|k}H_{k+1}^T(H_{k+1}P_{k+1|k}H_{k+1}^T+V_{k+1})^{-1}$$

$$\hat{x}_{k+1|k+1} = \hat{x}_{k+1|k}+K_{k+1}(y_{k+1}-\hat{y}_{k+1|k})$$

$$P_{k+1|k+1} = (I-K_{k+1}H_{k+1})P_{k+1|k}(I-K_{k+1}H_{k+1})^T+K_{k+1}V_{k+1}K_{k+1}^T$$

其中，$H_{k+1}$为测量转移矩阵，$V_{k+1}$为测量噪声协方差矩阵，$y_{k+1}$为第$k+1$时刻的测量值，$\hat{y}_{k+1|k}$为预测值，$K_{k+1}$为卡尔曼增益矩阵，$\hat{x}_{k+1|k+1}$为状态估计值，$P_{k+1|k+1}$为状态估计协方差矩阵。