def test_covariance_propagation(self):
####Not Important Place Holders####
p0 = np.zeros((3, 1))
q0 = np.zeros((3, 1))
v0 = np.zeros((3, 1))
ba0 = np.zeros((3, 1))
bg0 = np.zeros((3, 1))
cov0 = np.identity(15)
belief = StatesCovariance(p0, q0, v0, ba0, bg0, cov0)
dp = np.zeros((3, 1))
dq = np.zeros((3, 1))
dv = np.zeros((3, 1))
dba = np.zeros((3, 1))
dbg = np.zeros((3, 1))
dynamics = np.concatenate((dp, dq, dv, dba, dbg), axis=0)
ft = DynamicModel(dynamics)
####
RProcess = np.array([[1.0, 2.0, 3.0], [0.0, 1.0, -1.0],
[1.0, 1.0, -2.0]])
RImu = np.array([[0.2, 0.1, 0.3], [0.4, 1.0, 0.4], [0.1, 2.0, 1.1]])
At = np.array([[1.0, -1.0, 1.0], [2.0, 2.0, 0.0], [0.0, 1.0, 1.0]])
Bt = np.array([[1.2, -1.0, 0.2], [2.0, 0.0, 0.1], [0.0, 2.0, 1.0]])
P = np.array([[2.0, 1.0, 0.0], [0.0, 2.0, -1.0], [1.0, 0.0, -1.0]])
belief.P = P
dt = 0.1
ekf.propagate(belief, RProcess, RImu, ft, At, Bt, dt)
PExpected = np.array([[2.45348, 1.55738, 0.1122],
[0.06234, 3.21891, -1.0549],
[1.0546, 0.4899, -1.221]])
self.assert_states_equal(belief.P, PExpected)
class Service():
# model_name must be supplied.
# otherwise no configuration cad be loaded.
def __init__(self, model_name=None):
self.model_name = model_name
self.dynamic_model = DynamicModel(self.model_name)
def _get_train_data(self):
(x_train, y_train), (x_test, y_test) = mnist.load_data()
# reshape to be [samples][width][height][channels]
x_train = x_train.reshape(
(x_train.shape[0], 28, 28, 1)).astype('float32')
x_test = x_test.reshape((x_test.shape[0], 28, 28, 1)).astype('float32')
y_train = np_utils.to_categorical(y_train)
y_test = np_utils.to_categorical(y_test)
self.x_train = x_train
self.x_test = x_test
self.y_train = y_train
self.y_test = y_test
def train(self):
# Load data
self._get_train_data()
# This return the compiled Keras Model from dynamic_model->model()
print(self.y_train)
model = self.dynamic_model.model()
model.fit(self.x_train,
self.y_train,
batch_size=1000,
epochs=4,
verbose=1)
# Save trained model
now = datetime.datetime.now()
model.save(
f"src/models/{self.model_name}_{now.year}{now.month}{now.day}_{now.hour}{now.minute}.h5"
)
return True
def predict(self, X):
# Load model
model = self._load_model()
# Execute
results = model.predict(X)
if results is not None and results != False:
return results
return False
def imu_callback(self, imu):
if self.firstImu:
self.imuPrevTime = imu.time
self.firstImu = False
return
dt = imu.time - self.imuPrevTime
self.imuPrevTime = imu.time
ut = Inputs(comp_filter.run(self.baseStates, imu, dt, self.params.kp))
self.baseStates.update_w_lpf(ut.gyros)
ft = DynamicModel(ekf.update_dynamic_model(self.belief, ut))
At = ekf.update_jacobian_A(self.belief, ut)
Bt = ekf.update_jacobian_B(self.belief, ut)
ekf.propagate(self.belief, self.params.RProcess, self.params.RInputs,
ft, At, Bt, dt)
self.update_full_state(ut.phi, ut.theta)
def test_state_propagation(self):
p0 = np.array([[0.1, -0.2, 4.0]]).T
q0 = np.array([[-0.0, -2.2, 3.0]]).T
v0 = np.array([[2.2, 4.1, 2.2]]).T
ba0 = np.array([[0.1, 2.1, -1.0]]).T
bg0 = np.array([[8.4, 2.1, -2.0]]).T
cov0 = np.identity(15)
belief = StatesCovariance(p0, q0, v0, ba0, bg0, cov0)
RProcess = np.zeros((15, 15))
RImu = np.zeros((6, 6))
dp = np.array([[1.2, -0.3, 0.0]]).T
dq = np.array([[-0.1, -2.1, 1.0]]).T
dv = np.array([[3.4, 1.1, 2.0]]).T
dba = np.array([[4.3, 0.0, -0.0]]).T
dbg = np.array([[3.2, 3.0, 0.22]]).T
dynamics = np.concatenate((dp, dq, dv, dba, dbg), axis=0)
ft = DynamicModel(dynamics)
At = np.identity(15)
Bt = np.zeros((15, 6))
dt = 0.1
ekf.propagate(belief, RProcess, RImu, ft, At, Bt, dt)
pExpected = np.array([[0.22, -0.23, 4.0]]).T
qExpected = np.array([[-0.01, -2.41, 3.1]]).T
vExpected = np.array([[2.54, 4.21, 2.4]]).T
baExpected = np.array([[0.53, 2.1, -1.0]]).T
bgExpected = np.array([[8.72, 2.4, -1.978]]).T
beliefExpected = StatesCovariance(pExpected, qExpected, vExpected,
baExpected, bgExpected, cov0, cov0,
cov0, cov0, cov0)
self.assert_states_equal(belief, beliefExpected)
def test_zeros(self):
p0 = np.zeros((3, 1))
q0 = np.zeros((3, 1))
v0 = np.zeros((3, 1))
ba0 = np.zeros((3, 1))
bg0 = np.zeros((3, 1))
cov0 = np.identity(15)
belief = StatesCovariance(p0, q0, v0, ba0, bg0, cov0)
RProcess = np.zeros((15, 15))
RImu = np.zeros((6, 6))
dp = np.zeros((3, 1))
dq = np.zeros((3, 1))
dv = np.zeros((3, 1))
dba = np.zeros((3, 1))
dbg = np.zeros((3, 1))
dynamics = np.concatenate((dp, dq, dv, dba, dbg), axis=0)
ft = DynamicModel(dynamics)
At = np.zeros((15, 15))
Bt = np.zeros((15, 6))
dt = 0.1
ekf.propagate(belief, RProcess, RImu, ft, At, Bt, dt)
pExpected = np.zeros((3, 1))
qExpected = np.zeros((3, 1))
vExpected = np.zeros((3, 1))
baExpected = np.zeros((3, 1))
bgExpected = np.zeros((3, 1))
beliefExpected = StatesCovariance(pExpected, qExpected, vExpected,
baExpected, bgExpected, cov0)
self.assert_states_equal(belief, beliefExpected)
self.assert_covariances_equal(belief.P, beliefExpected.P)
def __init__(self, model_name=None):
self.model_name = model_name
self.dynamic_model = DynamicModel(self.model_name)
input_std = np.std(model_input, axis=0)
label_mean = np.mean(model_label, axis=0)
label_std = np.std(model_label, axis=0)
# tensorflow session
sess = tf.Session()
# initialize dynamic model
dynamic_model = DynamicModel(sess,
save_dir,
input_size,
output_size,
input_mean,
input_std,
label_mean,
label_std,
num_layers,
num_layer_nodes,
batch_size,
learning_rate,
dtype=tf.float64)
# initialize all tensorflow variables
sess.run(tf.global_variables_initializer())
# make saver
saver = tf.train.Saver(max_to_keep=0)
print("\n##################################################")
print("Training the dynamic model")
from dynamic_model import ModelParam, DynamicModel, ModelState
print_sim_time = False
plot_visualization = True
plot_states = True
# create parameter struct
param = ModelParam()
# initial state
x0 = ModelState()
hover_omega = np.sqrt(0.5 * (param.m1 + param.m2 + param.m3 + 2 * param.mp) * param.g / param.c_t)
# x0.beta_dot = np.array([hover_omega, -hover_omega])
# instantiate model
model = DynamicModel(param, x0)
# simulation time step
dt = 0.05
# prepare simulation
max_sim_time = 2.5
sim_time = 0
sim_time_vec = [sim_time]
state_vec = [copy.copy(model.state)]
start_time = time.time()
if plot_visualization:
fig = plt.figure(0)
ax = fig.add_subplot(111, projection='3d')