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()
