Commit e1248de3 authored by Joe Watson's avatar Joe Watson
Browse files

tweak params and add history

parent 648942e6
......@@ -46,7 +46,7 @@ class QubeBase(gym.Env):
mu = np.zeros((dim_x,))
sigma = 1e-6 * np.eye(dim_x)
sigma_process = 1e-6 * np.eye(dim_x)
sigma_observation = 1e-5 * np.eye(2)
sigma_observation = 1e-6 * np.eye(2)
C = np.concatenate((np.eye(2), np.zeros((2, 2))), axis=1)
self.filter = CubatureKalman(QubeDynamicsVectorized(1 / fs), C, 4,
mu, sigma, sigma_process, sigma_observation)
......@@ -70,7 +70,7 @@ class QubeBase(gym.Env):
for _ in range(self.timing.n_sim_per_ctrl):
a_cmd = self._lim_act(x, a)
x = self._sim_step(a_cmd)
# x = self.filter(a_cmd, x[:2])
x = self.filter(a_cmd, x[:2])
return x, a_cmd # return the last applied (clipped) command
def _rwd(self, x, a):
......@@ -207,8 +207,14 @@ class QubeDynamics:
class QubeDynamicsVectorized(QubeDynamics):
def __init__(self, dt):
super().__init__()
self.dt = dt
self.Mr = 0.095 * 2.
# self.Dr = 1e-5
# self.Dr = 0.0015 # viscous damping (N-m-s/rad), original: 0.0015
# self.Dp = 0.0005 # viscous damping (N-m-s/rad), original: 0.0005
self._init_const()
def __call__(self, xu):
th = xu[:, 0]
......
......@@ -68,10 +68,18 @@ class CubatureKalman(object):
self.sig_obs = sig_obs
assert sig_process.shape == (d_x, d_x)
self.inference = QuadratureInference(Quadrature(1, 0, 0), d_x)
self.mu_history = []
self.sig_history = []
self.y_history = []
def reset(self):
self.mu_x = np.copy(self._mu_x)
self.sig_x = np.copy(self._sig_x)
self.mu_history = []
self.sig_history = []
self.y_history = []
self.mu_history.append(self.mu_x)
self.sig_history.append(self.sig_x)
def __call__(self, u, y):
x_pts = self.inference.get_x_pts(self.mu_x, self.sig_x)
......@@ -85,4 +93,9 @@ class CubatureKalman(object):
K = (np.linalg.solve(sig_y, self.C) @ sig_x2).T
self.mu_x = (mu_x2 + K @ (y - mu_y))[None, :]
self.sig_x = (np.eye(self.d_x) - K @ self.C) @ sig_x2
self.mu_history.append(self.mu_x)
self.sig_history.append(self.sig_x)
self.y_history.append(y)
return self.mu_x.reshape((-1,))
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment