srkf

Arguments

f, h

current system matrices

q, r

covariance matrices of dynamics and observation noise

x0, p0

state estimate and error variance at t=0 based on data up to t=-1

y

current observation Output

x1, p1

updated estimate and error covariance at t=1 based on data up to t=0

Description

This function is the square root form of Kalman filter. It is more efficient than the simple Kalman filter in term of numerical stability, especially if dynamic noise covariance q is small. In effect, that can provock an indefinite numerical representation of the state covariance matrix p, whereas p is positive definite. So, the goal of srkf is to propagate p using a Cholesky factorization algorithm. These factors can be updated at each step of the algorithm, which allows to keep p in its basic form.

Example

f=[0 0 1; 0 1 0; 2 3 4]; //State matrix
g=[1;-1;1]; //Input matrix
h=[1 1 0; 0 1 0; 0 0 0]; //Output matrix
Q=[3 2 1; 2 2 1; 1 1 1]; //Covariance matrix of dynamic noise
R=[2 1 1; 1 1 1; 1 1 2]; //Covariance matrix of observation noise
// Initialisation
p0=[6 3 2; 3 5 2; 2 2 4];
x0=[1;1;1];
y=[2 8 7]'; // Current observation output matrix
[x1,p1]=srkf(y,x0,p0,f,h,Q,R)

See also

• kalm — Kalman update
• sskf — steady-state Kalman filter