EKF – Extended (nonlinear) Kalman filter

Block SymbolLicensing group: MODEL
PIC

Function Description
The block implements a nonlinear state estimator known as Extended Kalman filter. The goal is to provide estimates of unmeasurable state quantities of a nonlinear dynamic system described by a state space model dxdt = f(x,u) + w(t),y = h(x,u) + v(t) for a continuous-time case and x(k + 1) = f(x(k),u(k)) + w(k),y(k) = h(x(k),u(k)) + v(k) for the case of a discrete-time system. The variables w,v are the process and observation noises which are both assumed to be zero mean multivariate Gaussian processes with covariance Q and R specified in the block parameters. The Extended Kalman filter is the nonlinear version of the Kalman filter which linearizes the state and output equations about the current working point. It is a predictor-corrector type algorithm which switches between open-loop prediction using the state equation and correction of the estimates by directly measured output quantities. The measurements can be supplied to the filter non-equidistantly in an arbitrary execution period of the block.

The prediction step is run in each execution period and solves the state equation by numerical integration, starting from an initial value x0 and initial covariance P0. Various numerical methods, chosen by the user specified parameter solver, are available to perform the integration of the vector state differential equation. A special choice of solver = 1 signalizes the discrete-time system case for which the numerical integration reduces to simple evaluation of the recursive formula given by the first-order difference equation in x(k + 1) = f(x(k),u(k)). Apart from the state vector, also its covariance matrix P is propagated in time, capturing the uncertainty of the estimates in the form of their (co)variances. Please refer to the documentation of the NSSM block for more details about the available numerical integration algorithms.

The filtering correction step takes place whenever the input of the block is set to nz > 0. This signalizes that new vector of measurements is available at the z input and it is used to correct the state and its covariance estimates from the prediction step. Multiple right sides of the output equation can be implemented in the cooperating REXLANG block. This may be useful e.g. for systems equipped with various sensors providing their data asynchronously to each other (and with respect to the block execution times) with different sampling periods. For the setting nz = 0, the user algorithm signalizes no output data available in the current execution period, forcing the filter to extrapolate the state estimates by performing the prediction step only.

The Extended Kalman filter is generally not an optimal filter in the sense of minimization of the mean-squared error of the obtained state estimates. However, it provides modest performance for sufficiently smooth nonlinear systems and is considered to be a de facto standard solution for nonlinear estimation. A special case is obtained by setting linear state and output equations in the cooperating REXLANG block. This case leads to standard linear Kalman filter which is stochastically optimal for the formulated state estimation problem.

Inputs

funcRef

Cooperating REXLANG block reference

Reference

u

Input vector of the model

Reference

z

Output (mesurement) vector of the model

Reference

nz

Index of the actual output vector set   1

Long (I32)

Qk

State noise covariance matrix

Reference

Rk

Output noise covariance matrix

Reference

RST

Block reset

Bool

HLD

Hold

Bool

x0

Initial state vector

Reference

P0

Initial covariance matrix

Reference

Parameters

nmax

Allocated size of output matrix (total number of items)   5  10000 20

Long (I32)

solver

Numeric integration method  2

Long (I32)

1 ....

Discrete equation

2 ....

Euler (1st order)

3 ....

2nd order Adams-Bashforth

4 ....

3rd order Adams-Bashforth

5 ....

4th order Adams-Bashforth

6 ....

5th order Adams-Bashforth

7 ....

4th order Runge-Kutha

8 ....

Implicit Euler

9 ....

Implicit Euler(more iteration)

10 ...

2nd order Adams-Moulton implicit

11 ...

2nd order Adams-Moulton implicit (more iterations)

12 ...

3rd order Adams-Moulton implicit

13 ...

3nd order Adams-Moulton implicit (more iterations)

14 ...

2nd order RadauIIA implicit

15 ...

2nd order RadauIIA implicit (more iterations)

16 ...

3rd order RadauIIA implicit

17 ...

3rd order RadauIIA implicit (more iterations)

Outputs

x

Model state vector

Reference

P

Model state covariance matrix

Reference

trP

Trace of model state covariance matrix

Reference

cmd

Cooperating REXLANG block requested function

Long (I32)

f

Vector reference set by cooperating REXLANG block

Reference

df

Matrix reference set by cooperating REXLANG block

Reference

err

Error code (0 is OK, see SystemLog for details)

Long (I32)

2024 © REX Controls s.r.o., www.rexygen.com