Daniel@0: function [x,y] = sample_lds(F, H, Q, R, init_state, T, models, G, u)
Daniel@0: % SAMPLE_LDS Simulate a run of a (switching) stochastic linear dynamical system.
Daniel@0: % [x,y] = switching_lds_draw(F, H, Q, R, init_state, models, G, u)
Daniel@0: % 
Daniel@0: %   x(t+1) = F*x(t) + G*u(t) + w(t),  w ~ N(0, Q),  x(0) = init_state
Daniel@0: %   y(t) =   H*x(t) + v(t),  v ~ N(0, R)
Daniel@0: %
Daniel@0: % Input:
Daniel@0: % F(:,:,i) - the transition matrix for the i'th model
Daniel@0: % H(:,:,i) - the observation matrix for the i'th model
Daniel@0: % Q(:,:,i) - the transition covariance for the i'th model
Daniel@0: % R(:,:,i) - the observation covariance for the i'th model
Daniel@0: % init_state(:,i) - the initial mean for the i'th model
Daniel@0: % T - the num. time steps to run for
Daniel@0: %
Daniel@0: % Optional inputs:
Daniel@0: % models(t) - which model to use at time t. Default = ones(1,T)
Daniel@0: % G(:,:,i) - the input matrix for the i'th model. Default = 0.
Daniel@0: % u(:,t)   - the input vector at time t. Default = zeros(1,T)
Daniel@0: %
Daniel@0: % Output:
Daniel@0: % x(:,t)    - the hidden state vector at time t.
Daniel@0: % y(:,t)    - the observation vector at time t.
Daniel@0: 
Daniel@0: 
Daniel@0: if ~iscell(F)
Daniel@0:   F = num2cell(F, [1 2]);
Daniel@0:   H = num2cell(H, [1 2]);
Daniel@0:   Q = num2cell(Q, [1 2]);
Daniel@0:   R = num2cell(R, [1 2]);
Daniel@0: end
Daniel@0: 
Daniel@0: M = length(F);
Daniel@0: %T = length(models);
Daniel@0: 
Daniel@0: if nargin < 7,
Daniel@0:   models = ones(1,T);
Daniel@0: end
Daniel@0: if nargin < 8,
Daniel@0:   G = num2cell(repmat(0, [1 1 M]));
Daniel@0:   u = zeros(1,T);
Daniel@0: end
Daniel@0: 
Daniel@0: [os ss] = size(H{1});
Daniel@0: state_noise_samples = cell(1,M);
Daniel@0: obs_noise_samples = cell(1,M);
Daniel@0: for i=1:M
Daniel@0:   state_noise_samples{i} = sample_gaussian(zeros(length(Q{i}),1), Q{i}, T)';
Daniel@0:   obs_noise_samples{i} = sample_gaussian(zeros(length(R{i}),1), R{i}, T)';
Daniel@0: end
Daniel@0: 
Daniel@0: x = zeros(ss, T);
Daniel@0: y = zeros(os, T);
Daniel@0: 
Daniel@0: m = models(1);
Daniel@0: x(:,1) = init_state(:,m);
Daniel@0: y(:,1) = H{m}*x(:,1) + obs_noise_samples{m}(:,1);
Daniel@0: 
Daniel@0: for t=2:T
Daniel@0:   m = models(t);
Daniel@0:   x(:,t) = F{m}*x(:,t-1) + G{m}*u(:,t-1) + state_noise_samples{m}(:,t);
Daniel@0:   y(:,t) = H{m}*x(:,t)  + obs_noise_samples{m}(:,t);
Daniel@0: end
Daniel@0: 
Daniel@0: