Home > src > ghkf_update.m

# ghkf_update

## PURPOSE GHKF_UPDATE - Gauss-Hermite Kalman filter update step

## SYNOPSIS function [M,P,K,MU,S,LH] = ghkf_update(M,P,Y,h,R,h_param,p)

## DESCRIPTION ``` GHKF_UPDATE - Gauss-Hermite Kalman filter update step

Syntax:
[M,P,K,MU,S,LH] = GHKF_UPDATE(M,P,Y,h,R,param,p)

In:
M  - Mean state estimate after prediction step
P  - State covariance after prediction step
Y  - Measurement vector.
h  - Measurement model function as a matrix H defining
linear function h(x) = H*x, inline function,
function handle or name of function in
form h(x,param)
R  - Measurement covariance
h_param - Parameters of h
p  - Degree of approximation (number of quadrature points)

Out:
M  - Updated state mean
P  - Updated state covariance
K  - Computed Kalman gain
MU - Predictive mean of Y
S  - Predictive covariance Y
LH - Predictive probability (likelihood) of measurement.

Description:
Perform additive form Gauss-Hermite Kalman filter (GHKF)
measurement update step. Assumes additive measurement
noise.

Function h(.) should be such that it can be given a
DxN matrix of N sigma Dx1 points and it returns
the corresponding measurements for each sigma
point. This function should also make sure that
the returned sigma points are compatible such that
there are no 2pi jumps in angles etc.

Example:
h = inline('atan2(x(2,:)-s(2),x(1,:)-s(1))','x','s');
[M2,P2] = ghkf_update(M1,P1,Y,h,R,S);