# Mid-State Kalman Filter for Nonlinear Problems
## 5 Simulation  
### Problem Analysis  
When the wide-band radar tracks a very long-range target, a typical problem caused by the measurement nonlinearity is the \"Contact Lens\" problem. It is directly related to the filter update process and is the leading cause of filter inconsistency and divergence. The wide-band radar has high range accuracy and low angle accuracy when tracking very long-range targets. The uncertainty region of measurement will present a severe non-Gaussian distribution when the state in the Cartesian coordinate system is updated with nonlinear measurements from different coordinate systems. The uncertainty region presents a curved shape, similar to a banana shape in two dimensions and a contact lens shape in three dimensions. Therefore, this distribution is called the \"Contact Lens\" distribution and this kind of nonlinear problem is called the CL problem [23]. Figure 3 describes the reason for the uncertainty region of measurement from a geometric point of view. In the case of constant range accuracy, the range of cross-angle error \(R\sigma_{\theta}\) increases with the radar detection range. As it is far greater than the range error \(\sigma_{r}\), the uncertainty region of measurement gradually presents a curve. It should be noticed that in order to describe the distribution characteristics, the curve segment of constant thickness is used in Figure 3 to emphasize the overall curvature of the uncertainty region. However, the curvature of the actual space varies from place to place. The curvature is related to the probability of the measurement distribution at that point in the space. In Figure 4, the intensity of the color is proportional to the probability of the measurement distribution in the space. [PERSON] uses a parametric expression to describe the uncertainty region \(CL(x;r,\sigma_{r}^{2},\sigma_{\theta}^{2})\), but he did not give the specific expression of the distribution. The bias significance metric is put forward to measure the degree of its distortion from a Gaussian distribution [24]:  
\[\beta=\frac{r\delta_{\theta}^{2}}{2\delta_{r}} \tag{36}\]  
where \(r\) is the distance from the target to the radar, \(\delta_{\theta}^{2}\) is the radar angle variance, and \(\delta_{r}^{2}\) is the radar range variance. Figure 5 shows the measurement distribution characteristics corresponding to different values of \(\beta\) when the target is \(500\) km away from the radar. When \(\beta\) is small, the measurement distribution shows a typical Gaussian distribution and the traditional filter can track targets well. However, as the value increases, the nonlinear degree of measurement gradually increases. At this time, the nonlinear filters EKF and UKF will have consistency problems, resulting in the decrease of filtering accuracy and even the problem of filter divergence. This problem can be explained by theoretical analysis. Figure 4: The non-Gaussian CL error distribution. Figure 3: The CL Problem of Nonlinear Measurement. The EKF needs to linearize the measurement equation in Equation (10). Assume \((x,y)\) is the position coordinate of the target relative to the Cartesian coordinate system with the radar as the origin. The vector \((x_{0},y_{0})\) is an initial unbiased estimate of \((x,y)\) with the covariance matrix \(P_{0}\). The range and angle observation model of the radar is \(r=\left(x^{2}+y^{2}\right)^{1/2}+v_{r}\), \(\theta=\arctan(y/x)+v_{\theta}\). Updating \((x_{0},y_{0})\) with the measurement \(r\) and \(\theta\) through the EKF, obtains a new estimate \((x_{e},y_{e})\) with a covariance matrix \(P_{e}\). The EKF valuation equation is then expressed as  
\[\left[ \begin{array}{c}x_{e}\\ y_{e}\end{array} \right]=\left[ \begin{array}{c}x_{0}\\ y_{0}\end{array} \right]+P_{e}H_{0}^{T}R^{-1}\left[ \begin{array}{c}r-r_{0} \\ \theta-\theta_{0}\end{array} \right] \tag{37}\]  
where  
\[P_{e}^{-1}=P_{0}^{-1}+H_{0}^{T}R^{-1}H_{0} \tag{38}\]  
\[r_{0}=\left( \begin{array}{c}x_{0}^{2}+y_{0}^{2}\end{array}