class KGD: def __init__(self, x0, dx0, sigma_Q=0.01, sigma_R=2.0): #setup filter: self._N=len(x0) N=self._N z_0=np.hstack([x0, dx0]).reshape(2*N,1) Q=sigma_Q*np.eye(2*N)#dynamics noise R=sigma_R*np.eye(N)#observation noise C=np.hstack([np.zeros((N,N)), np.eye(N)]) self.kf=Kalman(init_state=z_0, init_cov=np.eye(2*N)*0.01, C_t=C, Q_t=Q, R_t=R ) def step(self, x, dx, alpha=0.01, callback=None): N=self._N A_t=np.block( [[np.eye(N) , -alpha*np.eye(N)], # [np.zeros((N,N)) , 1-alpha*ddx]] [np.zeros((N,N)) , np.eye(N)]] ) z_hat = self.kf.filter(dx.T.reshape(N,1), A_t=A_t) x_hat, dx_hat=z_hat[:N], z_hat[N:] if callback: callback(x, t, dx_hat.reshape(x.shape)) # x+=-alpha*dx_hat.flatten() return x - alpha*dx_hat.flatten()
def _rmsprop_filtered(g, x, callback=None, num_iters=200, alpha=lambda t: 0.1, gamma=lambda t: 0.9, eps=1e-8, sigma_Q=0.01, sigma_R=2.0): """ Implements the Kalman RMSProp algorithm from https://arxiv.org/pdf/1810.12273.pdf. """ # Initialize the Kalman filter N = len(x) dx = g(x) r = np.ones(len(x)) #should be ones. z_0 = np.hstack([r, x, dx]).reshape(3 * N, 1) Q = sigma_Q * np.eye(3 * N) #dynamics noise R = sigma_R * np.eye(N) #observation noise C = np.hstack([np.zeros((N, 2 * N)), np.eye(N)]) kf = Kalman(init_state=z_0, init_cov=np.eye(3 * N) * 0.01, C_t=C, Q_t=Q, R_t=R) for t in range(num_iters): # evaluate the gradient dx = g(x) #construct transition matrix beta_t = alpha(t) / np.sqrt((gamma(t) * r + (1 - gamma(t)) * (dx**2)) + eps) A_t = np.block([[ np.eye(N) * gamma(t), np.zeros((N, N)), (1 - gamma(t)) * np.diag(dx) ], [np.zeros((N, N)), np.eye(N), -beta_t * np.eye(N)], [np.zeros((N, N)), np.zeros((N, N)), np.eye(N)]]) #increment the kalman filter z_hat = kf.filter(dx.T.reshape(N, 1), A_t=A_t) r_hat, x_hat, dx_hat = z_hat[:N], z_hat[N:2 * N], z_hat[2 * N:] #perform the Kalman RMSProp update r = gamma(t) * r + (1 - gamma(t)) * dx_hat.flatten()**2 x += -alpha(t) / (np.sqrt(r) + eps) * dx_hat.flatten() if callback: callback(x, t, dx_hat.reshape(x.shape)) return x
def _momgd_filtered(g, x, callback=None, num_iters=200, alpha=lambda t: 0.1, mu=lambda t: 0.9, sigma_Q=0.01, sigma_R=2.0): """ Implements the Kalman Gradient Descent + momentum algorithm from https://arxiv.org/pdf/1810.12273.pdf. """ # Initialize the Kalman filter N = len(x) dx = g(x) v = np.zeros(len(x)) z_0 = np.hstack([v, x, dx]).reshape(3 * N, 1) Q = sigma_Q * np.eye(3 * N) #dynamics noise R = sigma_R * np.eye(N) #observation noise C = np.hstack([np.zeros((N, 2 * N)), np.eye(N)]) kf = Kalman(init_state=z_0, init_cov=np.eye(3 * N) * 0.01, C_t=C, Q_t=Q, R_t=R) for t in range(num_iters): # evaluate the gradient dx = g(x) #construct transition matrix A_t = np.block( [[np.eye(N) * mu(t), np.zeros((N, N)), (1 - mu(t)) * np.eye(N)], [ -alpha(t) * np.eye(N), np.eye(N), -alpha(t) * (1 - mu(t)) * np.eye(N) ], [np.zeros((N, N)), np.zeros((N, N)), np.eye(N)]]) #increment the kalman filter z_hat = kf.filter(dx.T.reshape(N, 1), A_t=A_t) v_hat, x_hat, dx_hat = z_hat[:N], z_hat[N:2 * N], z_hat[2 * N:] if callback: callback(x, t, dx_hat.reshape(x.shape)) #perform the KGD + momentum update v = mu(t) * v - (1 - mu(t)) * dx_hat.flatten( ) #don't flatte, use .reshpape(x.shape) x += alpha(t) * v return x
class KRMSProp: """ A stateful version of Kalman RMSProp from https://arxiv.org/pdf/1810.12273.pdf which is used in distributed Kalman RMSProp. """ def __init__(self, x0, dx0, sigma_Q=0.01, sigma_R=2.0): self._N = len(x0) N = self._N r = np.ones(len(x0)) #should be ones. z_0 = np.hstack([r, x0, dx0]).reshape(3 * N, 1) Q = sigma_Q * np.eye(3 * N) #dynamics noise R = sigma_R * np.eye(N) #observation noise C = np.hstack([np.zeros((N, 2 * N)), np.eye(N)]) self.kf = Kalman(init_state=z_0, init_cov=np.eye(3 * N) * 0.01, C_t=C, Q_t=Q, R_t=R) self._r = r def step(self, x, dx, callback=None, alpha=0.01, gamma=0.9, eps=1e-8): r = self._r N = self._N beta_t = alpha / np.sqrt((gamma * r + (1 - gamma) * (dx**2) + eps)) A_t = np.block( [[np.eye(N) * gamma, np.zeros((N, N)), (1 - gamma) * np.diag(dx)], [np.zeros((N, N)), np.eye(N), -beta_t * np.eye(N)], [np.zeros((N, N)), np.zeros((N, N)), np.eye(N)]]) z_hat = self.kf.filter(dx.T.reshape(N, 1), A_t=A_t) r_hat, x_hat, dx_hat = z_hat[:N], z_hat[N:2 * N], z_hat[2 * N:] r = gamma * r + (1 - gamma) * dx_hat.flatten()**2 self._r = r return x - alpha / (np.sqrt(r) + eps) * dx_hat.flatten()
# Initialize kalman Filter kalman = Kalman(x0, P0, F, H, Q, R) # initialize measurement measure = Measurement(0, 80, dt) # Temporary state values and measurement values measured_speeds = [] positions = [] speeds = [] # Perform measurement for the time period for index, k in enumerate(time_peride): Z = measure.measure() # train speed measurement X = kalman.filter(Z) # train position prediction measured_speeds.append(Z[0][0]) positions.append(X[0][0]) speeds.append(X[1][0]) # Error covariance for position kalman_P_list = [P[1][1] for P in kalman.P_list] # Kalman gain for position kalman_K_list = [K[1][0] for K in kalman.K_list] # Plot results plt.plot(time_peride, measured_speeds, color='gray', linestyle=':') plt.plot(time_peride, speeds) plt.xlabel('Measurement samples [sample/s]')