Run one update of a basic extended information filter. Though less common than the extended Kalman filter, thi filter can be faster where the dimension of the measurement is larger than the dimension of the state, and has the advantage that it can start without any knowledge of the state, where the information matrix contains all zeros (corresponding to infinite covariance).
x_k = F_km1 * x_km1 + G_km1 * w_q
z_k = H_k * x_k + w_r
where km1
means k1
, w_q
is a zeromean Gaussian random variable
with zero mean and covariance Q_km1
. That is, w_q ~ N(0, Q_km1)
.
Similarly, w_r ~ N(0, R_k)
.
It updates a state estimate and covariance from sample k1 to sample k.
[x_k, Y_k] = eif(t_km1, t_k, x_km1, Y_km1, u_km1, z_k, ...
f, inv_F_km1, G_km1, inv_Q_km1, ...
h, H_k, inv_R_k);
t_km1  Time at sample k1 

t_k  Time at sample k 
x_km1  State estimate at sample k1 
Y_km1  State information matrix at sample k1 
u_km1  Input vector at sample k1 
z_k  Measurement vector at sample k 
f  Propagation function

inv_F_km1  Either the inverse of the state transition matrix at sample k1 or a function to compute the inverse of the state transition matrix with the following interface:

G_km1  Map from process noise to state 
inv_Q_km1  Either the inverse of the process noise covariance at sample k1 or a function to compute it, with the following interface:

h  Propagation function

H_k  Either the observation matrix at sample k or a function to compute the observation matrix with the following interface:

inv_R_k  Either the inverse of the measurement noise covariance matrix at sample k or a function to compute it, with the following interface:

Outputs:
x_k  Updated state estimate for sample k 

Y_k  Updated information matrix for sample k 
Similar to the example in the lkf
function, we can quickly create a
simulation for discrete, dynamic system, generate noisy measurements of
the system over time, and pass these to an information filter.
First, define the discrete system.
rng(1);
dt = 0.1; % Time step
F_km1 = expm([0 1; 1 0]*dt); % State transition matrix
H_k = [1 0]; % Observation matrix
Q_km1 = 0.5^2; % Process noise variance
G_km1 = [0.5*dt^2; dt]; % Processnoisetostate map
R_k = 0.1; % Meas. noise covariance
Information filters work with inverses of many of these matrices (compared to Kalman filters).
inv_F = inv(F_km1);
inv_Q = inv(Q_km1);
inv_R = inv(R_k);
Make propagation and observation functions. These are just linear for
this example, but eif
would work fine with nonlinear functions.
f = @(t_km1, t_k, x_km1, u_km1) F_km1 * x_km1;
h = @(t_k, x_k, u_k) H_k * x_k;
Now, we'll define the simulation's time step and initial conditions. Note that we define the initial estimate and set the truth as a small error from the estimate (using the covariance).
n = 100; % Number of samples to simulate
x_hat_0 = [1; 0]; % Initial estimate
P = diag([0.5 1].^2); % Initial estimate covariance
Y = inv(P); % Initial information matrix
x_0 = x_hat_0 + mnddraw(P, 1); % Initial true state
Now we'll just write a loop for the discrete simulation.
% Storage for time histories
x = [x_0, zeros(2, n1)]; % True state
x_hat = [x_hat_0, zeros(2, n1)]; % Estimate
z = [H_k * x_0 + mnddraw(R_k, 1), zeros(1, n1)]; % Measurement
% Simulate each sample over time.
for k = 2:n
% Propagate the true state.
x(:, k) = F_km1 * x(:, k1) + G_km1 * mnddraw(Q_km1, 1);
% Create the real measurement at sample k.
z(:, k) = H_k * x(:, k) + mnddraw(R_k, 1);
% Run the Kalman correction.
[x_hat(:,k), Y] = eif((k1)*dt, k*dt, x_hat(:,k1), Y, [], z(:,k), ...
f, inv_F, G_km1, inv_Q, h, H_k, inv_R);
end
Plot the results.
figure(1);
clf();
t = 0:dt:(n1)*dt;
h = plot(t, x, ...
t, z, '.', ...
t, x_hat, '');
legend('True x1', 'True x2', 'Meas.', 'Est. x1', 'Est. x2');
xlabel('Time');
Note how similar this example is to the first example of lkf
. The
difference is the inverted matrices that this uses and the use of G_km1
with a 1dimensional process noise which exactly matches the Q_km1
matrix used in the lkf
example.
BarShalom, Yaakov, X. Rong Li, and Thiagalingam Kirubarajan. Estimation with Applications to Tracking and Navigation. New York: John Wiley & Sons, Inc., 2001. Print. Pages 303306.
*kf
v1.0.3