Home > src > kf_update.m

# kf_update

## PURPOSE

KF_UPDATE Kalman Filter update step

## SYNOPSIS

function [X,P,K,IM,IS,LH] = kf_update(X,P,y,H,R)

## DESCRIPTION

```KF_UPDATE  Kalman Filter update step

Syntax:
[X,P,K,IM,IS,LH] = KF_UPDATE(X,P,Y,H,R)

In:
X - Nx1 mean state estimate after prediction step
P - NxN state covariance after prediction step
Y - Dx1 measurement vector.
H - Measurement matrix.
R - Measurement noise covariance.

Out:
X  - Updated state mean
P  - Updated state covariance
K  - Computed Kalman gain
IM - Mean of predictive distribution of Y
IS - Covariance or predictive mean of Y
LH - Predictive probability (likelihood) of measurement.

Description:
Kalman filter measurement update step. Kalman Filter
model is

x[k] = A*x[k-1] + B*u[k-1] + q,  q ~ N(0,Q)
y[k] = H*x[k]   + r,             r ~ N(0,R)

Prediction step of Kalman filter computes predicted
mean m-[k] and covariance P-[k] of state:

p(x[k] | y[1:k-1]) = N(x[k] | m-[k], P-[k])

See for instance KF_PREDICT how m-[k] and P-[k] are
calculated.

Update step computes the posterior mean m[k] and
covariance P[k]  of state given new measurement:

p(x[k] |�y[1:k]) = N(x[k] |�m[k], P[k])

Innovation distribution is defined as

p(y[k] |�y[1:k-1]) = N(y[k] | IM[k], IS[k])

Updated mean x[k] and covarience P[k] are given by
the following equations (not the only possible ones):

v[k] = y[k] - H[k]*m-[k]
S[k] = H[k]*P-[k]*H[k]' + R[k]
K[k] = P-[k]*H[k]'*[S[k]]^(-1)
m[k] = m-[k] + K[k]*v[k]
P[k] = P-[k] - K[k]*S[k]*K[k]'

Example:
m = m0;
P = P0;
M = m0;
for i=1:size(Y,2)
[m,P] = kf_predict(m,P,A,Q);
[m,P] = kf_update(m,P,Y(:,i),H,R);
M = [M m];
end

KF_PREDICT, EKF_UPDATE```

## CROSS-REFERENCE INFORMATION

This function calls:
• gauss_pdf GAUSS_PDF Multivariate Gaussian PDF
This function is called by:
• imm_filter IMM_FILTER Interacting Multiple Model (IMM) Filter prediction and update steps
• imm_smooth IMM_SMOOTH Fixed-interval IMM smoother using two IMM-filters.
• imm_update IMM_UPDATE Interacting Multiple Model (IMM) Filter update step
• kf_loop KF_LOOP Performs the prediction and update steps of the Kalman filter
• tf_smooth TF_SMOOTH Two filter based Smoother
• uimm_smooth UIMM_SMOOTH UKF based Fixed-interval IMM smoother using two IMM-UKF filters.
• uimm_update IMM_UPDATE UKF based Interacting Multiple Model (IMM) Filter update step

Generated on Fri 12-Aug-2011 15:08:47 by m2html © 2005