def kalman_filter(self, observ): """Kalman filter using the model on a set of observations""" # Get system matrices F = self.transition_matrix() Q = self.transition_covariance() H = self.observation_matrix() R = self.observation_covariance() # Initialise arrays of Gaussian densities and (log-)likelihood num_time_instants = len(observ) flt = GaussianDensityTimeSeries(num_time_instants, self.ds) prd = GaussianDensityTimeSeries(num_time_instants, self.ds) lhood = 0 # Loop through time instants for kk in range(num_time_instants): # Prediction if kk > 0: prd_kk = kal.predict(flt.get_instant(kk-1), F, Q) else: prd_kk = self.initial_state_prior prd.set_instant(kk, prd_kk) # Correction - handles misisng data indicated by NaNs y = observ[kk] if not np.any(np.isnan(y)): # Nothing missing - full update flt_kk,innov = kal.correct(prd.get_instant(kk), y, H, R) lhood = lhood + mvn.logpdf(observ[kk], innov.mn, innov.vr) elif np.all(np.isnan(y)): # All missing - no update flt_kk = prd_kk else: # Partially missing - delete missing elements missing = np.where( np.isnan(y) ) yp = np.delete(y, missing, axis=0) Hp = np.delete(H, missing, axis=0) Rp = np.delete(np.delete(R, missing, axis=0), missing, axis=1) flt_kk,innov = kal.correct(prd.get_instant(kk), yp, Hp, Rp) lhood = lhood + mvn.logpdf(yp, innov.mn, innov.vr) flt.set_instant(kk, flt_kk) return flt, prd, lhood
def rts_smoother(self, flt, prd): """Rauch-Tung-Striebel smooth using the model""" # Get system matrices F = self.transition_matrix() # Initialise arrays of Gaussian densities and (log-)likelihood num_time_instants = flt.num_time_instants smt = GaussianDensityTimeSeries(num_time_instants, self.ds) # Loop through time instants for kk in reversed(range(num_time_instants)): # RTS update if kk < num_time_instants-1: smt_kk = kal.update(flt.get_instant(kk), smt.get_instant(kk+1), prd.get_instant(kk+1), F) else: smt_kk = flt.get_instant(kk) smt.set_instant(kk, smt_kk) return smt
def rts_smoother(self, flt, prd): """Rauch-Tung-Striebel smooth using the model""" # Get system matrices F = self.transition_matrix() # Initialise arrays of Gaussian densities and (log-)likelihood num_time_instants = flt.num_time_instants smt = GaussianDensityTimeSeries(num_time_instants, self.ds) # Loop through time instants for kk in reversed(range(num_time_instants)): # RTS update if kk < num_time_instants - 1: smt_kk = kal.update(flt.get_instant(kk), smt.get_instant(kk + 1), prd.get_instant(kk + 1), F) else: smt_kk = flt.get_instant(kk) smt.set_instant(kk, smt_kk) return smt
def kalman_filter(self, observ): """Kalman filter using the model on a set of observations""" # Get system matrices F = self.transition_matrix() Q = self.transition_covariance() H = self.observation_matrix() R = self.observation_covariance() # Initialise arrays of Gaussian densities and (log-)likelihood num_time_instants = len(observ) flt = GaussianDensityTimeSeries(num_time_instants, self.ds) prd = GaussianDensityTimeSeries(num_time_instants, self.ds) lhood = 0 # Loop through time instants for kk in range(num_time_instants): # Prediction if kk > 0: prd_kk = kal.predict(flt.get_instant(kk - 1), F, Q) else: prd_kk = self.initial_state_prior prd.set_instant(kk, prd_kk) # Correction - handles misisng data indicated by NaNs y = observ[kk] if not np.any(np.isnan(y)): # Nothing missing - full update flt_kk, innov = kal.correct(prd.get_instant(kk), y, H, R) lhood = lhood + mvn.logpdf(observ[kk], innov.mn, innov.vr) elif np.all(np.isnan(y)): # All missing - no update flt_kk = prd_kk else: # Partially missing - delete missing elements missing = np.where(np.isnan(y)) yp = np.delete(y, missing, axis=0) Hp = np.delete(H, missing, axis=0) Rp = np.delete(np.delete(R, missing, axis=0), missing, axis=1) flt_kk, innov = kal.correct(prd.get_instant(kk), yp, Hp, Rp) lhood = lhood + mvn.logpdf(yp, innov.mn, innov.vr) flt.set_instant(kk, flt_kk) return flt, prd, lhood