## 3 State Estimation by EKF  
To use the EKF method, the system needs to be linearized first. Assuming that the optimal estimate of \(\mathbf{X}_{j}\) is \(\mathbf{X}_{s}\), the corresponding state Equation can be written according to the EKF,  
\[\mathbf{x}_{j}=\mathbf{A}_{s}\mathbf{x}_{j-\mathbf{1}}+\mathbf{w}_{j-\mathbf{1}} \tag{10}\]  
where \(\mathbf{x}_{j}=\mathbf{X}_{j}-\mathbf{X}_{s}\), \(\mathbf{A}_{s}\) is the transition matrix that can be obtained in the linearization process, and \(\mathbf{w}_{j}\) is the system noise. The corresponding measurement Equation can be written as  
\[\mathbf{z}_{j}=\mathbf{H}\mathbf{x}_{j}+\mathbf{v}_{j} \tag{11}\]  
where \(\mathbf{H}\) is the measurement matrix and \(\mathbf{v}_{j}\) is the measurement noise. Both system noise and measurement noise are assumed to be zero-mean Gaussian white noise in the Kalman filter.  
Let \(\overline{\mathbf{X}}_{j}=\mathbf{X}_{j/j-\mathbf{1}}\) represent the prior estimate of the state variables at time \(j\) obtained using the optimal estimate at time \(j-1\). Let \(\hat{\mathbf{X}}_{j}=\mathbf{X}_{j/j}\) represent the optimal estimate of the state variables at time \(j\). Correspondingly:  
\[\overline{\mathbf{x}}_{j}=\overline{\mathbf{X}}_{j}-\mathbf{X}_{s},\ \hat{\mathbf{x}}_{j}=\hat{\mathbf{X}}_{j}-\mathbf{X}_{s}. \tag{12}\]  
In the following discussion, \(\overline{\mathbf{P}}_{j}\) is the variance matrix of the prior estimation error, \(\hat{\mathbf{P}}_{j}\) is the optimal estimation variance matrix (\(\hat{\mathbf{P}}_{0}\) is known), \(\mathbf{L}_{j}\) is the Kalman gain, and \(\mathbf{R}\) is the variance matrix of the measurement noise. The steps of applying EKF to estimate state variables are as follows [79; 80]:  
1. With the initial value \((\hat{\mathbf{X}}_{0},\hat{\mathbf{P}}_{0})\), linearize the Equations near \(\hat{\mathbf{X}}_{0}\) and obtain the transfer matrix \(\mathbf{A}_{s}\);
2. Solve the nonlinear Equations (5)-(7), as \(F(\mathbf{X}_{j},\mathbf{X}_{j-\mathbf{1}})=0\)\(P_{i,j}=P(x_{i},t_{j})\), and \(\overline{\mathbf{X}}_{j}\) can be computed and obtained: \(\overline{\mathbf{x}}_{j}=\overline{\mathbf{X}}_{j}-\mathbf{X}_{s}\); \(\overline{\mathbf{P}}_{j}=\mathbf{A}_{s}\overline{\mathbf{P}}_{j-\mathbf{1}}\mathbf{A}_{s}^{ \mathsf{T}}+\mathbf{Q}\);
3. Use the following formula to compute the Kalman gain: \(\mathbf{L}_{j}=\overline{\mathbf{P}}_{j}\mathbf{H}^{\mathsf{T}}[\mathbf{H}\overline{\mathbf{P}}_{j} \mathbf{H}^{\mathsf{T}}+\mathbf{R}]^{-1}\);
4. Obtain the optimal estimate \(\hat{\mathbf{X}}_{j}\) by measuring the value \(\mathbf{z}_{j}\) \[\hat{\mathbf{x}}_{j}=\overline{\mathbf{x}}_{j}+\mathbf{L}_{j}(\mathbf{z}_{j}-\mathbf{H}\mathbf{x}_{j}), \ \hat{\mathbf{X}}_{j}=\mathbf{X}_{s}+\hat{\mathbf{x}}_{j},\] obtain the optimal estimated variance matrix \(\hat{\mathbf{P}}_{j}\) at time \(j\) \[\hat{\mathbf{P}}_{j}=\overline{\mathbf{P}}_{j}-\mathbf{L}_{j}\mathbf{H}\overline{\mathbf{P}}_{j}\]obtain the optimal estimate \(\hat{\mathbf{X}}_{j}\) by measuring the value \(z_{j}\) \[\hat{\mathbf{x}}_{j}=\overline{\mathbf{x}}_{j}+\mathbf{L}_{j}(\mathbf{z}_{j}-\mathbf{H}\mathbf{x}_{j}), \ \hat{\mathbf{X}}_{j}=\mathbf{X}_{s}+\hat{\mathbf{x}}_{j},\] and obtain the optimal estimated variance matrix \(\hat{\mathbf{P}}_{j}\) at time \(j\) \[\hat{\mathbf{P}}_{j}=\overline{\mathbf{P}}_{j}-\mathbf{L}_{j}\mathbf{H}\overline{\mathbf{P}}_{j};\]
5. Go back to step 2 and calculate the optimal estimate of the moment \(j+1\). From the above computation, we can obtain the optimal estimation of the state vector \(\mathbf{X}_{j}\).