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 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 backward_simulation(self, flt): """Use backward simulation to sample from the state joint posterior""" # Get system matrices F = self.transition_matrix() Q = self.transition_covariance() + 1E-10*np.identity(self.ds) # Initialise sampled sequence num_time_instants = flt.num_time_instants x = np.zeros((num_time_instants, self.ds)) # Loop through time instatnts, sampling each state for kk in reversed(range(num_time_instants)): if kk < num_time_instants-1: samp_dens,_ = kal.correct(flt.get_instant(kk), x[kk+1], F, Q) else: samp_dens = flt.get_instant(kk) x[kk] = mvn.rvs(mean=samp_dens.mn, cov=samp_dens.vr) return x
def backward_simulation(self, flt): """Use backward simulation to sample from the state joint posterior""" # Get system matrices F = self.transition_matrix() Q = self.transition_covariance() + 1E-10 * np.identity(self.ds) # Initialise sampled sequence num_time_instants = flt.num_time_instants x = np.zeros((num_time_instants, self.ds)) # Loop through time instatnts, sampling each state for kk in reversed(range(num_time_instants)): if kk < num_time_instants - 1: samp_dens, _ = kal.correct(flt.get_instant(kk), x[kk + 1], F, Q) else: samp_dens = flt.get_instant(kk) x[kk] = mvn.rvs(mean=samp_dens.mn, cov=samp_dens.vr) return x
#u = np.array([0, 0, 1, 0, 0, 1]) #Q = np.eye(x.shape[0]) sq2 = 0.1**2 Q = np.array([[0.5 * (dt**2) * sq2, 0, 0, 0, 0, 0], [0, dt * sq2, 0, 0, 0, 0], [0, 0, sq2, 0, 0, 0], [0, 0, 0, 0.5 * (dt**2) * sq2, 0, 0], [0, 0, 0, 0, dt * sq2, 0], [0, 0, 0, 0, 0, sq2]]) R = 0.01 * np.eye(z.shape[0]) N = 100 predictions, corrections, measurements = [], [], [] for k in np.arange(0, N): x, P = kalman.predict(x, P, A, B, u, Q) predictions.append(x) x, P = kalman.correct(x, C, z, P, R) corrections.append(x) measurements.append(z) z = np.array([[x[0, 0] + random.uniform(0, 1)], [x[1, 0] + random.uniform(0, 1)]]) # test measurement measurements_plot_x = [] predictions_plot_x = [] corrections_plot_x = [] measurements_plot_y = [] predictions_plot_y = [] corrections_plot_y = [] for x in range(10): print("TEST: Predictions: {}".format(predictions[x][0][0]))
def fast_loop(print_lock, state_vector_lock, path_planning_lock, imu, enc, freq): clock = Clock() dt = 0 while True: start = time.time() A = iss.get_A(dt) # update state transition matrix Q = iss.get_Q(dt) # update covariance matrix a_x = imu.linear_acceleration[0] a_y = imu.linear_acceleration[1] theta = imu.euler[0] u = iss.get_u(a_x, a_y) # get control vector (actually this is for the next loop, but this was the best way of handling the state...) with state_vector_lock: # required so we lidar doesn't write at the same time x, P = kalman.predict(x, P, A, B, u, Q) # predict state from imu ticks_l, ticks_r = enc.get_ticks() # get 'measured' position with state_vector_lock: x, P = kalman.correct(x, C, z, P, R) with path_planning_lock: # run control code based on position estimate and current planned path with print_lock: print("This is dummy text") # read imu data (to give starting acceleration) # while True: # time since clock began = clock.time()? # state space model to estimate position from previous imu data (with time) # WITH STATE_VECTOR_LOCK: # predict belief of position vector, with covariance vector in KF # read imu data for this moment # read encoder ticks # state space model to estimate position, with covariance vector # WITH STATE_VECTOR_LOCK: # update belief of position vector in KF # WITH PATH_PLANNING_LOCK: # run control code based on current position estimate and planned path # actuate motors based on the control code time.sleep(0.01) clock.tick(freq) # limits the loop to "freq" fps - stops unnecessary processing power being used dt = time.time() - start # make sure this doesn't break / clash with clock.tick() def slow_loop(print_lock, text): freq = 10 clock = Clock() while True: with print_lock: print("This {} will be taken by the Lidar scanning and Path Planning".format(text)) time.sleep(0.01) clock.tick(freq) # while True: # listen for Lidar scan from arduino # if lidar_scan: # infer_position using one of the methods you know # WITH STATE_VECTOR_LOCK: # update belief of position vector in KF # run path planning code # WITH PATH_PLANNING_LOCK (and maybe state_vector_lock?): # update path plan based on current position and end goal def main(): imu = imu_sensor.IMU() enc = encoders.Encoders() fast_loop_freq = Config['pygame']['fast_loop_freq'] # 60 Hz loop speed print_lock = threading.Lock() state_vector_lock = threading.Lock() path_planning_lock = threading.Lock() fast_thread = threading.Thread(target=fast_loop, args=(print_lock, state_vector_lock, path_planning_lock, imu, enc, fast_loop_freq), name="fast_loop") fast_thread.start() slow_thread = threading.Thread(target=slow_loop, args=(print_lock, 'dummy_text'), name="slow_loop") slow_thread.start() if __name__ == "__main__": main()
H = np.eye(2) measurementNoise = 0.9 sensor = kalman.MeasurementModel(2*H,measurementNoise,z) kalman = kalman.KalmanFilter(np.zeros((2,2))) stateEstimates = [] detP = [] for i in range(1,N): oldState = modelFreeFall.state[i-1].reshape((2,1)) modelFreeFall.state[i] = np.transpose(modelFreeFall.calcNewState(oldState)) trueState = modelFreeFall.state[i].reshape((2,1)) measure = sensor.calcNewMeasurement(trueState) kalman.predict(oldState,modelFreeFall,sensor) kalman.calcKGain(sensor) stateEstimates.append(kalman.correct(sensor,measure)) detP.append(np.linalg.det(kalman.P)) z[i] = measure.reshape(2) t[i] = t[i-1] + dt detP = np.array(detP) stateEstimates = np.array(stateEstimates).reshape((99,2)) print detP.shape,t.shape ,np.transpose(stateEstimates)[0].shape plt.figure() #plt.scatter( t, np.transpose(sensor.z)[0]) plt.scatter(t,np.transpose(modelFreeFall.state)[0]) plt.scatter(t[1:100],np.transpose(stateEstimates)[0]) #plt.scatter(t[1:100],detP) plt.show()