Beispiel #1
0
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
Beispiel #2
0
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
Beispiel #3
0
class PersonEstimate(object):
    def __init__(self, msg):
        self.pos = msg
        self.reliability = 0.1
        self.k = Kalman()

    def update(self, msg):
        last = self.pos
        self.pos = msg
        self.reliability = max(self.reliability, msg.reliability)

        ivel = subtract(self.pos.pos, last.pos)
        time = (self.pos.header.stamp - last.header.stamp).to_sec()
        if time == 0:
            return
        scale(ivel, 1.0 / time)

        self.k.update([ivel.x, ivel.y, ivel.z])

    def age(self):
        return self.pos.header.stamp

    def id(self):
        return self.pos.object_id

    def velocity(self):
        k = self.k.values()
        if k == None:
            return Vector3()
        v = Vector3(k[0], k[1], k[2])
        return v

    def publish_markers(self, pub):
        gen.scale = [.1, .3, 0]
        gen.color = [1, 1, 1, 1]
        vel = self.velocity()
        #scale(vel, 15)
        m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)])
        m.header = self.pos.header
        pub.publish(m)

    def get_person(self):
        p = Person()
        p.name = self.id()
        p.position = self.pos.pos
        p.velocity = self.velocity()
        p.reliability = self.reliability
        p.tags.append(self.pos.name)
        return self.pos.header.frame_id, p
Beispiel #4
0
	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
			)
Beispiel #5
0
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()
Beispiel #6
0
	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
Beispiel #7
0
class PersonEstimate:
    def __init__(self, msg):
        self.pos = msg
        self.reliability = 0.1
        self.k = Kalman()

    def update(self, msg):
        last = self.pos
        self.pos = msg
        self.reliability = max(self.reliability, msg.reliability)

        ivel = subtract(self.pos.pos, last.pos)
        time = (self.pos.header.stamp - last.header.stamp).to_sec()
        scale(ivel, 1.0/time)
            
        self.k.update([ivel.x, ivel.y, ivel.z])

    def age(self):
        return self.pos.header.stamp

    def id(self):
        return self.pos.object_id

    def velocity(self):
        k = self.k.values()
        if k==None:
            return Vector3()
        v = Vector3(k[0],k[1],k[2])
        return v

    def publish_markers(self, pub):
        gen.scale = [.1, .3, 0]
        gen.color = [1,1,1,1]
        vel = self.velocity()
        #scale(vel, 15)
        m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)])
        m.header = self.pos.header
        pub.publish(m)

    def get_person(self):
        p = Person()
        p.name = self.id()
        p.position = self.pos.pos
        p.velocity = self.velocity()
        p.reliability = self.reliability
        return self.pos.header.frame_id, p
class BoatEstimate(object):
    def __init__(self, msg, stamp):
        self.det_boat = msg
        self.stamp = stamp
        self.k = Kalman()

    def update(self, msg, stamp):
        last = self.det_boat
        last_stamp = self.stamp
        self.det_boat = msg
        self.stamp = stamp

        ivel = subtract(self.det_boat.pose.position, last.pose.position)
        if ((self.stamp - last_stamp).to_sec() != 0.0):
            time = (self.stamp - last_stamp).to_sec()
            scale(ivel, 1.0 / time)

        self.k.update([ivel.x, ivel.y, ivel.z])

    def id(self):
        return self.det_boat.id

    def velocity(self):
        k = self.k.values()
        if k == None:
            return Vector3()
        v = Vector3(k[0], k[1], k[2])
        return v

    def get_boat(self):
        b = Boat()
        b.id = self.id()
        b.lidar_index = self.det_boat.lidar_index
        b.pose = self.det_boat.pose
        b.velocity = self.velocity()
        b.size = self.det_boat.size
        return b
Beispiel #9
0
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()
Beispiel #10
0
 def __init__(self, msg):
     self.pos = msg
     self.reliability = 0.1
     self.k = Kalman()
Beispiel #11
0
    H = np.array([[0, 1]])

    # Process noise
    Q = np.array([[1, 0], [0, 3]])

    # Measurement noise
    R = np.array([[10]])

    # Initial states x0=0m und v=20m/s
    x0 = np.array([[0, 20]]).transpose()

    # Initial error covariance matrix
    P0 = np.array(5 * np.eye(2))

    # 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
 def __init__(self, msg, stamp):
     self.det_boat = msg
     self.stamp = stamp
     self.k = Kalman()
Beispiel #13
0
 def __init__(self, msg):
     self.pos = msg
     self.reliability = 0.1
     self.k = Kalman()