function [ydot] = example_4_7_1_extended_kf_fn(t,y, b, H_A, R_A, Q_A)
%
% Inputs: 
%
% Written by:
% -- 
% John L. Weatherwax                2005-08-14
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

% initialize storage: 
ydot = zeros(length(y),1);

% extract out the state, x and the covariance, P:
x = y(1:3);
P = reshape(y(4:end),3,3);

% form the system matrix "F" involved in the state dynamic differential equation \dot{x} = F x:
%
F = zeros(3,3);
F(1,2) = 1; 
F(2,1) = x(3);
F(2,2) = b; 

ydot(1:3) = F * x; % the state update equation i.e. dot{x} = F x 

% form the matrix F_A used in the linearized Riccati equation (this is DIFFERENT than F above in a small way): 
% 
F_A = zeros(3,3);
F_A(1,2) = 1; 
F_A(2,1) = x(3);
F_A(2,2) = b; 
F_A(2,3) = x(1); % <- this is the difference

L_A = zeros(3,1); 
L_A(2) = -x(3);

Pdot = F_A * P + P * F_A' + L_A * Q_A * L_A' - P * (H_A') * ( R_A \ eye(2) ) * H_A * P; % the covariance update i.e. dot{P}

ydot(4:end) = Pdot(:);