Extended Kalman Filter - 2023.1 English

Vitis Libraries
Release Date
2023-12-20
Version
2023.1 English

The Kalman filter estimates the state vector in a linear model. If the model is nonlinear, then a linearization procedure is performed to obtain the filtering equations. The Kalman filter so obtained will be called the Extended Kalman filter. A state-space description of non-linear system can have a non-linear model of the form:



7.073625 x k + 1 = ( x k ) + T k ( x k ) ξ k



6.754124999999998 z k =   h k ( x k ) + η k


Where fk and hk are valued functions with ranges in Rn and Rq, respectively. 1≤q≤n, and Tk a matrix-valued function with range in RnxRq such that for each k the first order partial derivatives of fk (x:sub:k) and hk (x:sub:k) with respect to all the components of xk are continuous. We consider zero-mean Gaussian white noise sequences image122 and image123 with ranges in Rp and Rq respectively, 1≤p, q≤n.

The real-time linearization process is carried out as shown in the following equations. In the lines of the linear model, the initial estimate and predicted position are chosen to be:

11.835 x ^ 0 = E ⎛ ⎝   x 0 ⎞ ⎠ ,   x ^ 1 | 0 = f 0 ⎛ ⎝   x ^ 0 ⎞ ⎠


Then, 8.436824999999999 x ^ k = x ^ k | k , consecutively, for k=1,2,…, use the predicted positions.

11.835 x ^ k | k - 1 = f k - 1 ⎛ ⎝ x ^ k - 1 ⎞ ⎠


Note

  1. 28.615470000000002 f k ( x k ) = ⎡ ⎣ ⎢ ⎢ f k 1 ( x k ) ⋮ f k n ( x k ) ⎤ ⎦ ⎥ ⎥ , where 28.615469999999995 x k = ⎡ ⎣ ⎢ ⎢ x k 1 ⋮ x k n ⎤ ⎦ ⎥ ⎥ , k is a time index and superscript is row index and 50.40188999999998 ⎡ ⎣ ∂ f k (   x k ) ∂   x k   ⎤ ⎦ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ∂ f k 1 (   x k ) ∂ x k 1 … ∂ f k 1 (   x k ) ∂ x k n ⋮ … ⋮ ∂ f k n (   x k ) ∂ x k 1 … ∂ f k n (   x k ) ∂ x k n ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥
  2. 3 R m is a space of column vectors 6.599699999999999 x = [ x 1 … . x m ] T
The equation for time update computations is as follows:

26.025 P k | k - 1 = ⎡ ⎣ ⎢ ⎢ ∂ f k - 1 ⎛ ⎝ x ^ k - 1 ⎞ ⎠ ∂ x k - 1 ⎤ ⎦ ⎥ ⎥ P k - 1 | k - 1 ⎡ ⎣ ⎢ ⎢ ∂ f k - 1 ⎛ ⎝ x ^ k - 1 ⎞ ⎠ ∂ x k - 1 ⎤ ⎦ ⎥ ⎥ T +   T k - 1 ⎛ ⎝ x ^ k - 1 ⎞ ⎠ Q k - 1 T k - 1 ⎛ ⎝ x ^ k - 1 ⎞ ⎠ T


17.13495 = F k - 1 P k - 1 | k - 1 F k - 1 T +   T k - 1 ⎛ ⎝ x ^ k - 1 ⎞ ⎠ Q k - 1 T k - 1 ⎛ ⎝ x ^ k - 1 ⎞ ⎠ T
The equation for measurement update computations is as follows:

image134



11.835 x ^ k | k = x ^ k | k - 1 + G k ⎛ ⎝ v k - h k ⎛ ⎝ x ^ k | k - 1 ⎞ ⎠ ⎞ ⎠



26.025 P k | k = ⎛ ⎝ ⎜ ⎜ I - G k ⎡ ⎣ ⎢ ⎢ ∂ h k ⎛ ⎝ x ^ k | k - 1 ⎞ ⎠ ∂ x k ⎤ ⎦ ⎥ ⎥ ⎞ ⎠ ⎟ ⎟ P k | k - 1 = ⎛ ⎝ I - G k H k ⎞ ⎠ P k | k - 1