# Mid-State Kalman Filter for Nonlinear Problems
## 3 Mid-State Kalman Filter for Nonlinear Problems
### Mid-State Kalman Filter  
Consider a nonlinear discrete system for target tracking as shown below:  
\[\left\{ \begin{array}{l}x_{k}=\mathbf{F}_{k}x_{k-1}+w_{k-1}\\ z_{k}=\mathbf{H}_{k}(x_{k})+v_{k}\end{array} \right. \tag{10}\]  
where, \(x_{k}\in\mathbf{R}^{\text{n}}\) and \(z_{k}\in\mathbf{R}^{\text{m}}\) are the system state vector and the measurement vector; \(\mathbf{F}_{k}(\cdot):\mathbf{R}^{\text{n}}\rightarrow\mathbf{R}^{\text{n}}\) and \(\mathbf{H}_{k}(\cdot):\mathbf{R}^{\text{n}}\rightarrow\mathbf{R}^{\text{m}}\) respectively represent the system state function and nonlinear measurement function; and the process noise \(w_{k}\) and observation noise \(v_{k}\) are independent zero-mean Gaussian noise with covariance \(\mathbf{Q}_{k}\) and \(\mathbf{R}_{k}\). In the typical two-dimensional radar target tracking, the target is assumed to be in uniform linear motion. Its state vector at time \(k\) is \(x_{k}=[x_{k},v_{x_{k}},y_{k},v_{y_{k}}]^{T}\), including the position \([x_{k},y_{k}]^{T}\) and velocity vector \([v_{x_{k}},v_{y_{k}}]^{T}\). In the observation vector \(z_{k}=[r_{k},\theta_{k}]^{T}\), \(r_{k}\) and \(\theta_{k}\) are the distance measurement and angle measurement, respectively. The system state function and nonlinear measurement function in Equation (10) are respectively expressed as:  
\[\mathbf{F}=\left[ \begin{array}{cccc}1&T&0&0\\ 0&1&0&0\\ 0&0&1&T\\ 0&0&0&1\end{array} \right],\mathbf{H}_{k}(x_{k})=\left[ \begin{array}{c}\sqrt {x_{k}^{2}+y_{k}^{2}}\\ \arctan(\frac{y_{k}^{2}}{x_{k}^{2}})\end{array} \right] \tag{11}\]  
The MSKF selects \(x_{M_{k}}=[z_{k},\dot{z}_{k}]^{T}=[r_{k},\dot{r}_{k},\theta_{k},\dot{\theta}_ {k}]^{T}\) as mid-state vector and linearizes the measurement equation in the measurement space as:  
\[z_{k}=\mathbf{H}_{z}x_{M_{k}}+v_{k}=\left[ \begin{array}{cc}I_{m}&0_{m\times m }\end{array} \right]\times[z_{k},\dot{z}_{k}]^{T}+v_{k} \tag{12}\]  
In this case, the nonlinear system in Equation (10) can be transformed into the following linear system:  
\[\left\{ \begin{array}{l}x_{k}=\mathbf{F}_{k}x_{k-1}+w_{k-1}\\ z_{k}=\mathbf{H}_{z}x_{M_{k}}+v_{k}\end{array} \right. \tag{13}\]  
This avoids the linearization process of the measurement function \(\mathbf{H}_{k}(x_{k})\). Because the system satisfies the linear Gaussian condition of the KF, the mid-state can be updated by the KF. As the optimal linear filter, KF has less information loss and higher filtering accuracy. In the mid-state, \([\dot{r}_{k},\dot{\theta}_{k}]^{T}\) is not only the first-order state derivative of \([r_{k},\theta_{k}]^{T}\), but also has practical physical significance. \(\dot{r}_{k}\) and \(\dot{\theta}_{k}\), respectively, represent the radial velocity \(v_{r}\) and angular velocity \(\omega\) of the target movement in polar coordinates. Therefore, the linear velocity is \(v_{\theta}=\omega r\). As shown in Figure 1, the relative radar distance of the target is \(r\), the angle is \(\theta\), and the velocity at time \(k\) is \(v\). In the cartesian coordinate system, the velocity vector \(v\) can be decomposed into \(x\) axial velocity \(v_{x}\) and \(y\) axial velocity \(v_{y}\). In the polar coordinate system, the velocity vector \(v\) can be decomposed into radial velocity \(v_{r}\) and linear velocity \(v_{\theta}\). It should be noted that the radial velocity \(v_{r}\) only changes the magnitude of the velocity \(v\) without changing its direction, while the linear velocity \(v_{\theta}\) only changes the direction of the velocity \(v\) without changing its magnitude. The mid-state is introduced into the Kalman filter to form the tracking filter algorithm MSKF, which is suitable for dealing with nonlinear problems. The algorithm steps are given as follows. **Initialization:** Given the initial state estimate \(\hat{x}_{0}\) and covariance matrix \(\mathbf{P}_{0}\), set the time \(k=1\).