# Mid-State Kalman Filter for Nonlinear Problems
## 5 Simulation
### Problem Analysis
\right)^{1/2} \tag{39}\]  
\[\theta_{0}=\arctan(y_{0}/x_{0}) \tag{40}\]  
\[H_{0}=\left[ \begin{array}{cc}\cos\theta_{0}&\sin\theta_{0}\\ -\sin\theta_{0}/r_{0}&\cos\theta_{0}/r_{0}\end{array} \right] \tag{41}\]  
\[R=\left[ \begin{array}{cc}\sigma_{r}^{2}&0\\ 0&\sigma_{\theta}{}^{2}\end{array} \right] \tag{42}\]  
Carrying out the matrix multiplication operation of Formula (37), we can get  
\[\left[ \begin{array}{c}x_{e}\\ y_{e}\end{array} \right]=\left[ \begin{array}{c}x_{0}\\ y_{0}\end{array} \right]+P_{e}\left[ \begin{array}{c}\cos\theta_{0}\\ \sin\theta_{0}\end{array} \right][(r-r_{0})/\sigma_{r}^{2}]+P_{e}\left[ \begin{array} []{c}-\sin\theta_{0}\\ \cos\theta_{0}\end{array} \right][(\theta-\theta_{0})/r_{0}\sigma_{\theta}^{2}] \tag{43}\]  
For high-resolution wide-band radar, \(P_{0}^{-1}\ll R^{-1}\), so \(P_{0}^{-1}\) can be ignored, then  
\[P_{e}\approx H_{0}^{-1}R(H_{0}^{T})^{-1} \tag{44}\]  
Thus,  
\[\left[ \begin{array}{c}x_{e}\\ y_{e}\end{array} \right]\approx\left[ \begin{array}{c}x_{0}\\ y_{0}\end{array} \right]+\left[ \begin{array}{c}\cos\theta_{0}\\ \sin\theta_{0}\end{array} \right](r-r_{0})+\left[ \begin{array}{c}-\sin\theta_ {0}\\ \cos\theta_{0}\end{array} \right]r_{0}(\theta-\theta_{0}) \tag{45}\]  
From the above formula,  
\[x_{e}=r\cos\theta_{0}-r_{0}(\theta-\theta_{0})\sin\theta_{0} \tag{46}\]\[y_{e}=r\sin\theta_{0}+r_{0}(\theta-\theta_{0})\cos\theta_{0} \tag{47}\]  
Therefore, the following formula can be obtained:  
\[r_{e}=\left(x_{e}^{2}+y_{e}^{2}\right)^{1/2} \tag{48}\]  
\[\theta_{e}=\arctan(y_{e}/x_{e}) \tag{49}\]  
Assuming that \((r-r_{0})/r\) can be ignored, then the estimation error caused by the linearization of the measurement equation in EKF is  
\[r_{e}-r\approx r[1+(\theta-\theta_{0})^{2}]^{1/2}-r \tag{50}\]  
\[\theta_{e}-\theta\approx-(\theta-\theta_{0})+\arctan(\theta-\theta_{0}) \tag{51}\]  
The above formula shows that when the target is far away from the radar, the estimation error may exceed the distance measurement error \(\sigma_{r}\), so the solution of the EKF will tend to diverge. The UKF is a numerical method to calculate the statistical characteristics of random variables after nonlinear transformation. Its essence is to use multiple deterministic sampling points to approximate the Gaussian probability density function. However, in the CL problem, the probability distribution of measurement in the Cartesian coordinate system obviously cannot be approximated simply by Gaussian distribution, so the filtering accuracy of the UKF is poor. Figure 6 shows the error ellipse of the EKF and UKF estimation results which shows that the Gaussian distribution estimated by the two filters cannot cover the measurement distribution well, so the performance of the two filters is degraded or even divergent. MSKF uses \(\left[x_{k},\dot{x}_{k},y_{k},y_{k}\right]^{\mathrm{T}}\) as the state vector as the prediction in Cartesian coordinates. The mid-state \([r,\dot{r},\theta,\dot{\theta}]\) is selected to make sure the update process is carried out in the measurement coordinates when updating. In the measurement space, the measurement distribution conforms to the Gaussian distribution assumption of the Kalman filter which ensures the consistency of the filter.