# Non-Gaussian Pseudolinear Kalman Filtering-Based Target Motion Analysis with State Constraints
## 4 PL-MMSE Kalman Filter with Nonlinear State Constraints
### Second-Order Approximation  
When the first and second-order terms are both kept, (40) can be rewritten into  
\[f(x) = \begin{bmatrix}x^{T}&1\end{bmatrix}\begin{bmatrix}M&m\\ m^{T}&m_{0}\end{bmatrix}\begin{bmatrix}x\\ 1\end{bmatrix} \] \[=x^{T}Mx+2m^{T}x+m_{0}\] \[=0. \tag{43}\]  
Here  
\[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\] \[m =(h^{\prime}(\hat{x}_{k|k})-\hat{x}_{k|k}^{T}h^{\prime\prime}( \hat{x}_{k|k}))^{T}/2,\] (45) \[m_{0} =h(\hat{x}_{k|k})-h^{\prime}(\hat{x}_{k|k})\hat{x}_{k|k}+(\hat{x} _{k|k})^{T}M\hat{x}_{k|k}-q. \tag{46}\]  
Construct an optimization problem by projecting an unconstrained state estimation onto a nonlinear constrained surface, i.e.,  
\[\tilde{x}=arg\min_{x}(z-Hx)^{T}(z-Hx)\quad s.t.\quad f(x)=0 \tag{47}\]  
The Lagrangian function is formed with the multiplier \(\lambda\) as  
\[J=(z-Hx)^{T}(z-Hx)+\lambda f(x). \tag{48}\]  
The optimal solution can be found by solving  
\[\frac{\partial J}{\partial x} =-H^{T}z+\lambda m+(H^{T}H+\lambda M)x\] \[=0, \tag{49}\] \[\frac{\partial J}{\partial\lambda} =x^{T}Mx+m^{T}x+x^{T}m+m_{0}\] \[=0. \tag{43}\]0  
Assume the matrix \(H^{T}H+\lambda M\) is invertible. The constrained solution \(\tilde{x}\) can be expressed by  
\[=0. \tag{43}\]1  
which is the unconstrained solution when \(\lambda=0\). Applying the Cholesky factorization to \(M\) and \(S=H^{T}H\) gives  
\[=0. \tag{43}\]2 \[=0. \tag{43}\]3where \(E\) is an upper right diagonal matrix. We can apply singular value decomposition (SVD) to the matrix \(LE^{-1}\) as  
\[=0. \tag{43}\]4  
where \(U\) and \(V\) are orthonormal matrices, and \(\Sigma\) is a diagonal matrix with its diagonal elements denoted by \(p_{i}\). In order to simplify (51), two additional vectors are defined as  
\[=0. \tag{43}\]5 \[=0. \tag{43}\]6 \[=0. \tag{43}\]7 \[=0. \tag{43}\]8  
With these new matrices and vector notations, (51) can be expressed as  
\[=0. \tag{43}\]9  
With (55), (56) and (57),  
\[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]0 \[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]1 \[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]2 \[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]3  
After plugging in (58) and (59), \(f(x)\) transforms into  
\[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]4 \[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]5 \[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]6 \[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]7  
Since (60) is a nonlinear equation of \(\lambda\), it is difficult to obtain a closed-form solution. Numerical root-finding algorithms are used such as [PERSON] method [40] to solve (60). The derivatives of \(f(\lambda)\) and \(e(\lambda)\) are  
\[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]8 \[M =\frac{1}{2}h^{\prime\prime}(\hat{x}_{k|k}), \tag{44}\]9 \[m =(h^{\prime}(\hat{x}_{k|k})-\hat{x}_{k|k}^{T}h^{\prime\prime}( \hat{x}_{k|k}))^{T}/2,\]0 \[m =(h^{\prime}(\hat{x}_{k|k})-\hat{x}_{k|k}^{T}h^{\prime\prime}( \hat{x}_{k|k}))^{T}/2,\]1  
with respect to \(\lambda\). Then, the iterative solution of \(\lambda\) with Newton method can be given by  
\[m =(h^{\prime}(\hat{x}_{k|k})-\hat{x}_{k|k}^{T}h^{\prime\prime}( \hat{x}_{k|k}))^{T}/2,\]2  
(63) starts with \(\lambda_{0}=0\). If \(|\lambda_{k+1}-\lambda_{k}|<\tau\) where \(\tau\) is the tolerance, or the number of iterations reaches a preset value, the iteration stops. Then, we can obtain the constrained state estimate of the moving target by substituting the solution of \(\lambda\) into (57). Table 3 shows the steps of the PL-MMSE with nonlinear constraints by second-order approximation. **Remark 1**. : _The algorithm presented in Table 1 have many potential applications, which use the model shown in (9) and (10). For example, in the ocean environment, a self-moving ship monitors noisy sonar bearings to an acoustic target ship and then pours the measurements into the filters to estimate and predict the source position and velocity [19].