% Define the system as a Taylor polynomial with x(0)=x0:
deg = 3;           % number of terms of the Taylor polynomial
x0=zeros(deg,1);   % initial state is all zeros
clear s
clear sav
clear tru
% Transition matrix:
h=0.05;          % stepsize
nilp = [zeros(deg-1,1), eye(deg-1); zeros(1,deg)];  % unit infinitesimal gen.
A = eye(deg);  % A = exp(h*nilp) generates the smooth semigroup
for i=1:deg-1
  A = A + (h * nilp)^i / factorial(i);
end
A(deg,deg)=0;  % last coordinate will not propagate
s.A = A;
% Define the measurement to be the function value:
H = [1,zeros(1,deg-1)];
% The highest-order term is unknown, so treat it as random N(0,1):
Q = zeros(deg,deg); Q(deg,deg)=1^2;     % variance, hence stdev^2
% Define a measurement error (stdev):
R = 1^2;    % variance, hence stdev^2

s.Q = Q;
s.H = H;
s.R = R;
% Do not define any system input (control) functions:
s.B = 0;
s.u = 0;
% Set the initial estimate and initial covariance
s.x = x0;             % initial state estimate
s.P = eye(deg,deg);   % i.i.d. covariates in the initial estimate
% Generate random highest-order derivatives and watch the filter operate.
tru=x0; % true values
sav(1)=tru(1);
for t=1:100
   tru = A*(tru + [zeros(deg-1,1); randn*2]);
   sav(end+1)=tru(1);
   s(end).z = H*tru+ R*randn; % create a measurement
   s(end+1)=kalmanf(s(end)); % perform a Kalman filter iteration
end
clf
hold on
grid on
% plot measurement data:
hz=plot([s(1:end-1).z],'r.');
% plot a-posteriori state estimates:
hk=plot(H*[s(2:end).x],'b-');
ht=plot(sav,'g-');
legend([hz hk ht],'observations','Kalman output','true value',0)
title('Taylor Polynomial Example')
hold off
