EKF algorithm

Consider the following set of state equations:

( + 1) = () + () cos(())
( + 1) = () + () sin(())
( + 1) = () + ()
Which in vectorial form becomes:

( + ) = ((), ())
And, being () and () the measured variables, collected in the vector ():

() = (())
The values of () and () are calculated from
() as follows:
+
=
2

=

The Jacobians are:

1 0 sin

= 0 1 cos
,
0 0 1

cos cos
2 2

= cos cos

, 2 2



1 0 0
=
, 0 1 0

However, neither () nor (|) are known. Lets suppose that we are at the time step , then the input data at
the current time step is:

()
(| 1)

()
(| 1)
First, the prediction error is computed:

(| 1))
() = () (
Which in turn is used to calculate a corrective factor:

() =
(|1)

() = (| 1) () [() (| 1) () + ()]1
Then, the current time state estimate is:

(|) =
(| 1) + ()()
Eventually, we need to provide the next time instant state and P matrix predictions:

(|) = ( () ()) (| 1)

(|) =
(|),()

( + 1|) =
(|), ()

( + 1|) = (|) (|) (|) + ()

Error
Prediction error estimation

Estimation
Current time estimation

Prediction
Next time step prediction