# Mid-State Kalman Filter for Nonlinear Problems
## 3 Mid-State Kalman Filter for Nonlinear Problems
### Mid-State Kalman Filter
**Prediction:** Assume that the input of filter in prediction step is the filtering result \(\hat{x}_{k}\) and \(\mathbf{P}_{k}\) at time \(k\), and obtain the state prediction result \(\hat{x}_{k+1|k}\) and \(\mathbf{P}_{k+1|k}\) through the following formula:  
\[\hat{x}_{k+1|k}=\mathbf{F}_{k}x_{k} \tag{14}\]  
\[\mathbf{P}_{k+1|k}=\mathbf{F}_{k}\mathbf{P}_{k}\mathbf{F}_{k}^{T}+\mathbf{Q}_ {k} \tag{15}\]  
**State transformation:** The According to the geometric relation and velocity decomposition in Figure 1, the transformation relationship from state vector \(\hat{x}_{k+1|k}=[x_{k+1|k},v_{x_{k+1|k}},y_{k+1|k},v_{y_{k+1|k}}]^{T}\) to mid-state vector \(x_{M_{k+1|k}}=[r_{M_{k+1|k}},\dot{r}_{M_{k+1|k}},\theta_{M_{k+1|k}},\dot{\theta}_ {M_{k+1|k}}]^{T}\) is shown in the following formula:  
\[\left\{ \begin{array}{l}r_{M_{k+1|k}}=\sqrt{x_{k+1|k}^{2}+y_{k+1|k} ^{2}}\\ \dot{r}_{M_{k+1|k}}=\frac{1}{r}(x_{k+1|k}v_{x_{k+1|k}}+y_{k+1|k}v_{y_{k+1|k}}) \\ \theta_{M_{k+1|k}}=\arctan(\frac{y_{k+1|k}}{x_{k+1|k}})\\ \dot{\theta}_{M_{k+1|k}}=\frac{1}{r^{2}}(x_{k+1|k}y_{x_{k+1|k}}-y_{k+1|k}v_{x_ {k+1|k}})\end{array} \right. \tag{16}\]  
The predicted covariance \(\textbf{P}_{k+1|k}\) in the state space was converted to the covariance \(\textbf{P}_{M_{k+1|k}}\) in the measurement space according to the third-order spherical-radial criterion:  
1. [label=(0)]
2. Obtain \(2n\) sampling points through \(\hat{x}_{k+1|k}\) and \(\textbf{P}_{k+1|k}\): \[\left\{ \begin{array}{l}w_{i}=w_{i+n_{x}}=\frac{1}{2n_{x}}\\ \zeta_{i}=\hat{x}_{k+1|k}+\sqrt{n_{x}\textbf{P}_{k+1|k}}e_{i}\\ \zeta_{i+n}=\hat{x}_{k+1|k}-\sqrt{n_{x}\textbf{P}_{k+1|k}}e_{i}\end{array}  \right.,\ i=1,2,\cdots,n_{x}\] (17) where \(e_{i}\) represents the unit vector with the \(i\)-th element being \(1\). 2. The value of the sampling point after conversion is \(\mathbb{C}_{i}=l(\zeta_{i}),\ i=1,2,\cdots,2n_{x}\). 3. Then the covariance \(\textbf{P}_{M_{k+1|k}}\) in the intermediate states in the measurement space is: \[\overline{c}=\sum_{i=1}^{2n}w_{i}\mathbb{C}_{i}\] (18) \[\textbf{P}_{M_{k+1|k}}=\sum_{i=1}^{2n}w_{i}(\mathbb{C}_{i}-\overline{c})( \mathbb{C}_{i}-\overline{c})^{T}\] (19)  
The state transformation represents the target state transformation relationship under different coordinates. This module can be quickly replaced according to the actual situation without modifying the filter prediction and update module. This also improves the practical engineering applicability of the MSKF. **Update:** The state estimation results \(x_{M_{k+1}}\) and \(\textbf{P}_{M_{k+1}}\) in the measurement space are obtained from the mid-states \(x_{M_{k+1|k}}\) and their covariance \(\textbf{P}_{M_{k+1|k}}\). \[\textbf{S}_{M_{k+1}}=\textbf{H}_{z}\textbf{P}_{M_{k+1|k}}\textbf{H}_{z}^{T}+ \textbf{R}_{k+1} \tag{20}\]  
Figure 1: Velocity decomposition. \[\mathbf{K}_{M_{k+1}}=\mathbf{P}_{M_{k+1|k}}\mathbf{H}_{z}\mathbf{S}_{M_{k+1|k}}^{ -1} \tag{21}\]  
\[x_{M_{k+1}}=x_{M_{k+1|k}}+\mathbf{K}_{M_{k+1}}(z_{k+1}-\mathbf{H}_{z}x_{M_{k+1|k}}) \tag{22}\]  
\[\mathbf{P}_{M_{k+1}}=\left(\mathbf{I}_{2m}-\mathbf{K}_{M_{k+1}}\mathbf{H}_{z} \right)\mathbf{P}_{M_{k+1|k}} \tag{23}\]  
where \(\mathbf{H}_{z}=\left[ \begin{array}{cc}I_{m}&0_{m\times m}\end{array} \right]\) is the observation matrix of the mid-state vector. **Extraction of state estimation:** Obtain the state estimation \(\hat{x}_{k+1}\) and \(\mathbf{P}_{k+1}\) at time \(k\) from the filtering results \(x_{M_{k+1}}\) and \(\mathbf{P}_{M_{k+1}}\) in measurement space. This process is the reverse process of state transformation. Therefore, the conversion relationship from the mid-state vector \(x_{M_{k+1}}=\left[r_{M_{k+1}},\dot{r}_{M_{k+1}},\theta_{M_{k+1}},\dot{\theta}_ {M_{k+1}}\right]^{T}\) to the state vector \(\hat{x}_{k+1}=\left[x_{k+1},v_{x_{k+1}},y_{k+1},v_{y_{k+1}}\right]^{T}\) is  
\[\left\{