%LTI_INT Integrate LTI ODE with Gaussian Noise
%
% Syntax:
% [x,P,A] = lti_int(x,P,F,L,Q,T)
%
% Description:
% Integrates LTI differential equation
%
% x' = F*x + L*w , w ~ N(0,Q)
%
% from t0=0 to t1=T or over given steps t0,t1,t2,t3,...
% Initial conditions can be in form
%
% x(t0) = x0
%
% or
%
% x(t0) ~ N(x0,P0)
%
% See also
% LTI_DISC
% History:
% 20.11.2002 The first official version.
%
% Copyright (C) 2002 Simo Särkkä
%
% $Id: lti_int.m 111 2007-09-04 12:09:23Z ssarkka $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.
function [x,P,A] = lti_int(x,P,F,L,Q,T)
%
% Check number of arguments
%
if nargin < 3
error('Too few arguments');
end
if nargin < 4
L = [];
end
if nargin < 5
Q = [];
end
if nargin < 6
T = [];
end
%
% Integrate one or many steps
%
if length(T) > 1
%
% Handle multiple time steps recursively
%
if isempty(P)
if isempty(x)
%
% No x or P given, return only A's
%
tA = eye(size(x,1));
A = zeros(size(x,1),size(x,1),length(T));
A(:,:,1) = tA;
for i=2:length(T)
[tx,tP,tA] = lti_int([],[],F,L,Q,T(i)-T(i-1));
A(:,:,i) = tA;
end
else
%
% Only x given, returns x's and A's
%
tx = x;
tA = eye(size(x,1));
x = zeros(size(x,1),length(T));
A = zeros(size(x,1),size(x,1),length(T));
x(:,1) = tx;
A(:,:,1) = tA;
for i=2:length(T)
[tx,tP,tA] = lti_int(tx,[],F,L,Q,T(i)-T(i-1));
x(:,i) = tx;
A(:,:,i) = tA;
end
end
else
%
% Both x and P given, return all
%
tx = x;
tP = P;
tA = eye(size(x,1));
x = zeros(size(x,1),length(T));
P = zeros(size(x,1),size(x,1),length(T));
A = zeros(size(x,1),size(x,1),length(T));
x(:,1) = tx;
P(:,:,1) = tP;
A(:,:,1) = tA;
for i=2:length(T)
[tx,tP,tA] = lti_int(tx,tP,F,L,Q,T(i)-T(i-1));
x(:,i) = tx;
P(:,:,i) = tP;
A(:,:,i) = tA;
end
end
else
%
% One step integration from 0 to T
%
if isempty(L)
L = eye(size(x,1));
end
if isempty(Q)
Q = zeros(size(x,1),size(x,1));
end
if isempty(T)
T = 1;
end
%
% Closed form integration of mean
%
A = expm(F*T);
x = A*x;
%
% Runge-Kutta Integration of Covariance
%
if ~isempty(P) & (nargout > 1)
f = inline('F*P+P*F''+L*Q*L','P','t','F','L','Q');
P = rk(f,P,0,T,F,L,Q);
end
end