( ) ( ) [ ] ( ) ( ) [ ]⎥ ⎦ ⎤ ⎢ ⎣ ⎡ + − + + − + = × π k /n θ n π k /n θ n n 2 1 sin 2 / sin sin 2 1 cos 2 / cos 2 cos (2 n) K K θ π θ θ π θ rm ( ) [ ] ( ) ( ) [ ] ( ) ( ) [ ]⎥ ⎦ ⎤ ⎢ ⎣ ⎡ + − + + − + − = × π k /n θ n π k /n θ n n r r n 2 1 sin 2 / sin sin 2 1 cos 2 / cos cos 2 1 K K θ π θ θ π θ ϕ ϕβ α r where n k 1,2,..., = . Appendix 2: Extended Kalman Filter Algorithm Extended Kalman Filter consists of two recursive steps: prediction and innovation. First, states are numerically integrated from the previous observation time to the current time. It starts with initial guess of states and error covariance matrix of states. Since the best estimates are available at subsequent steps, the best estimates serve as new initial guesses for next steps. [ ( )] ˆ 0 0|0 t E X X = and [(() ˆ )(() ˆ )] ˆ 0|0 0 0|0 0 0|0 T t t E P X X X X− − = States and covariance matrix are propagated from the previous step (k-1|k-1) to the current step (k|k-1) via state differential equations and linearized covariance differential equations: ⎪⎩ ⎪ ⎨ ⎧ + + = = − − − − 1| 1 1| 1 ˆ ( ) ( ) ( ) ( ) ( ), with initial condition ( ) ˆ with initial condition () ( (), (), ), k k T k k t t t t t t t t t t P P A P P A Q X X f X u & & Note the linear covariance propagation because A matrix is the Jacobian of nonlinear state differential equations, evaluated based on the predicted state at the current time step: | 1 ( ) − ∂ ∂ = k k t X f A The update step corrects the predicted states and covariance matrix based on Kalman gain Kk and innovation: 1 | 1 | 1 ) ( − − − + = K P H H P H R T k k k k T k k k k , )] ( [ ˆ | 1 | 1 | k k k k k k k k k t − − − + = X X K Y h X | 1 | ) ( ˆ − = − k k k k k k P I K H P where | 1− ∂ ∂ = k k k X h H These prediction and correction steps repeat until the final time. 449
RkJQdWJsaXNoZXIy MTMzNzEzMQ==