wolffd@0: function [xsmooth, Vsmooth, VVsmooth_future] = smooth_update(xsmooth_future, Vsmooth_future, ...
wolffd@0:     xfilt, Vfilt,  Vfilt_future, VVfilt_future, A, Q, B, u)
wolffd@0: % One step of the backwards RTS smoothing equations.
wolffd@0: % function [xsmooth, Vsmooth, VVsmooth_future] = smooth_update(xsmooth_future, Vsmooth_future, ...
wolffd@0: %    xfilt, Vfilt,  Vfilt_future, VVfilt_future, A, B, u)
wolffd@0: %
wolffd@0: % INPUTS:
wolffd@0: % xsmooth_future = E[X_t+1|T]
wolffd@0: % Vsmooth_future = Cov[X_t+1|T]
wolffd@0: % xfilt = E[X_t|t]
wolffd@0: % Vfilt = Cov[X_t|t]
wolffd@0: % Vfilt_future = Cov[X_t+1|t+1]
wolffd@0: % VVfilt_future = Cov[X_t+1,X_t|t+1]
wolffd@0: % A = system matrix for time t+1
wolffd@0: % Q = system covariance for time t+1
wolffd@0: % B = input matrix for time t+1 (or [] if none)
wolffd@0: % u = input vector for time t+1 (or [] if none)
wolffd@0: %
wolffd@0: % OUTPUTS:
wolffd@0: % xsmooth = E[X_t|T]
wolffd@0: % Vsmooth = Cov[X_t|T]
wolffd@0: % VVsmooth_future = Cov[X_t+1,X_t|T]
wolffd@0: 
wolffd@0: %xpred = E[X(t+1) | t]
wolffd@0: if isempty(B)
wolffd@0:   xpred = A*xfilt;
wolffd@0: else
wolffd@0:   xpred = A*xfilt + B*u;
wolffd@0: end
wolffd@0: Vpred = A*Vfilt*A' + Q; % Vpred = Cov[X(t+1) | t]
wolffd@0: J = Vfilt * A' * inv(Vpred); % smoother gain matrix
wolffd@0: xsmooth = xfilt + J*(xsmooth_future - xpred);
wolffd@0: Vsmooth = Vfilt + J*(Vsmooth_future - Vpred)*J';
wolffd@0: VVsmooth_future = VVfilt_future + (Vsmooth_future - Vfilt_future)*inv(Vfilt_future)*VVfilt_future;
wolffd@0: 
wolffd@0: