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