コード例 #1
0
    def GetTrueMeasurements(self, robot_pose):
        '''
        Get the true measurements for a robot at robot_pose using a noise-free
        range sensor.
        ===INPUT===
        robot_pose: of class RobotPose, the pose at which the robot measures
        ===OUTPUT===
        a list of Measurement
        '''
        
        if (not (isinstance(robot_pose, RobotPose))):
            raise FeatureBasedWorldException('robot_pose should be an instance of RobotPose.')

        m = []

        for feature in self.features:
            dx = feature.x - robot_pose.x 
            dy = feature.y - robot_pose.y
            
            d = np.sqrt(dx * dx + dy * dy)  # true distance
            phi = wrap_angle(np.arctan2(dy, dx) - robot_pose.theta) # true relative bearing
            
            m.append(Measurement(d, phi, feature.s))
            
        return m
コード例 #2
0
    def measurement_model(self, measurements, pose):
        '''
        Compute the probability of the particle and the measurements given
        ===INPUT===
        measurement: a list of Measurement
        pose: the proposed pose
        ===OUTPUT===
        a decimal representing the probability
        '''
        w = 1.0

        for m in measurements:
            j = m.s  # assuming the signature gives the correspondence

            fs = self.true_map.GetFeature(j)
            delta_x = fs.x - pose.x
            delta_y = fs.y - pose.y

            d = np.sqrt(delta_x * delta_x + delta_y * delta_y)
            b = np.arctan2(delta_y, delta_x) - pose.theta

            # deviation from expected measurement
            n_r = d - m.r
            n_phi = wrap_angle(b - m.phi)
            n_s = fs.s - m.s

            # compute probability of measurement.
            w = w * self.measurement_noise.GetProbability([n_r, n_phi, n_s])

        return w
コード例 #3
0
    def GetTrueMeasurements(self, robot_pose):
        '''
        Get the true measurements for a robot at robot_pose using a noise-free
        range sensor.
        ===INPUT===
        robot_pose: of class RobotPose, the pose at which the robot measures
        ===OUTPUT===
        a list of Measurement
        '''

        if (not (isinstance(robot_pose, RobotPose))):
            raise FeatureBasedWorldException(
                'robot_pose should be an instance of RobotPose.')

        m = []

        for feature in self.features:
            dx = feature.x - robot_pose.x
            dy = feature.y - robot_pose.y

            d = np.sqrt(dx * dx + dy * dy)  # true distance
            phi = wrap_angle(np.arctan2(dy, dx) -
                             robot_pose.theta)  # true relative bearing

            m.append(Measurement(d, phi, feature.s))

        return m
コード例 #4
0
    def measurement_model(self, measurements, pose):
        '''
        Compute the probability of the particle and the measurements given
        ===INPUT===
        measurement: a list of Measurement
        pose: the proposed pose
        ===OUTPUT===
        a decimal representing the probability
        '''
        w = 1.0

        for m in measurements:
            j = m.s # assuming the signature gives the correspondence
            
            fs = self.true_map.GetFeature(j)
            delta_x = fs.x - pose.x
            delta_y = fs.y - pose.y
            
            d = np.sqrt(delta_x * delta_x + delta_y * delta_y)
            b = np.arctan2(delta_y, delta_x) - pose.theta
            
            # deviation from expected measurement
            n_r = d - m.r
            n_phi = wrap_angle(b - m.phi)
            n_s = fs.s - m.s

            # compute probability of measurement. 
            w = w * self.measurement_noise.GetProbability([n_r, n_phi, n_s])

        return w
コード例 #5
0
    def measurement_update(self, measurement):
        rp = self.get_pose()

        # convert measurement into a matrix
        z = np.matrix([[measurement.r], \
                       [measurement.phi], \
                       [measurement.s]])
                       
        # line 10
        j = measurement.s # assuming the signature gives the correspondence

        # line 11
        fs = self.true_map.GetFeature(j)
        delta_x = fs.x - rp.x
        delta_y = fs.y - rp.y
        q = delta_x * delta_x + delta_y * delta_y

        # line 12, predicted measurement
        z_hat = np.matrix([[np.sqrt(q)], \
                           [wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], \
                           [fs.s]])

        # line 13
        H_t = np.matrix(np.eye(3))
        H_t[0, 0] = -delta_x / np.sqrt(q)
        H_t[0, 1] = -delta_y / np.sqrt(q)
        H_t[1, 0] =  delta_y / q
        H_t[1, 1] =  delta_x / q
        H_t[1, 2] = -1.0

        # line 14
        S = H_t * self.sigma * H_t.T + self.R

        # line 15
        K_t = self.sigma * H_t.T * np.linalg.inv(S)
        
        # line 16
        self.mu = self.mu + K_t * (z - z_hat)

        # line 17
        self.sigma = self.sigma - K_t * H_t * self.sigma
コード例 #6
0
    def measurement_update(self, measurement):
        rp = self.get_pose()

        # convert measurement into a matrix
        z = np.matrix([[measurement.r], \
                       [measurement.phi], \
                       [measurement.s]])

        # line 10
        j = measurement.s  # assuming the signature gives the correspondence

        # line 11
        fs = self.true_map.GetFeature(j)
        delta_x = fs.x - rp.x
        delta_y = fs.y - rp.y
        q = delta_x * delta_x + delta_y * delta_y

        # line 12, predicted measurement
        z_hat = np.matrix([[np.sqrt(q)], \
                           [wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], \
                           [fs.s]])

        # line 13
        H_t = np.matrix(np.eye(3))
        H_t[0, 0] = -delta_x / np.sqrt(q)
        H_t[0, 1] = -delta_y / np.sqrt(q)
        H_t[1, 0] = delta_y / q
        H_t[1, 1] = delta_x / q
        H_t[1, 2] = -1.0

        # line 14
        S = H_t * self.sigma * H_t.T + self.R

        # line 15
        K_t = self.sigma * H_t.T * np.linalg.inv(S)

        # line 16
        self.mu = self.mu + K_t * (z - z_hat)

        # line 17
        self.sigma = self.sigma - K_t * H_t * self.sigma
コード例 #7
0
    def measurement_update(self, measurement):
        '''
        update the state and covar estimate using the measurement
        c.f page 314, Probabilistic Robotics
        '''
        print("<---- measurement_update ---->")
        print(measurement)

        rp = self.get_pose()

        # convert measurement into a matrix
        z = measurement.ToMatrix()

        # line 8
        j = measurement.s  # assuming the signature gives the correspondence
        print("correspondence = " + str(j))

        # line 9
        if (not self.feature_seen[j]):
            f = FeatureState(rp.x + measurement.r * np.cos(measurement.phi + rp.theta), \
                             rp.y + measurement.r * np.sin(measurement.phi + rp.theta), \
                             measurement.s)
            # line 10, initialize feature state
            self.set_feature(j, f)
            self.feature_seen[j] = True

            print("newly observed " + str(f))

        fs = self.get_feature(j)

        # line 12
        delta_x = fs.x - rp.x
        delta_y = fs.y - rp.y

        # line 13
        q = delta_x * delta_x + delta_y * delta_y
        #~ print("delta_x = " + str(delta_x) + " delta_y = " + str(delta_y) + " q= " + str(q))

        # line 14, predicted measurement
        z_hat = np.matrix([[np.sqrt(q)], \
                           [wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], \
                           [fs.s]])
        #~ print("z_hat = ")
        #~ print(z_hat)

        # line 15
        F_xj = self.get_feature_selection_matrix(j)

        # line 16
        h_t_i = np.matrix(np.zeros((3, 6)))
        h_t_i[0, 0] = -delta_x * np.sqrt(q)
        h_t_i[0, 1] = -delta_y * np.sqrt(q)
        h_t_i[0, 3] = delta_x * np.sqrt(q)
        h_t_i[0, 4] = delta_y * np.sqrt(q)
        h_t_i[1, 0] = delta_y
        h_t_i[1, 1] = -delta_x
        h_t_i[1, 2] = -q
        h_t_i[1, 3] = -delta_y
        h_t_i[1, 4] = delta_x
        h_t_i[2, 5] = q
        H_t = h_t_i * F_xj / q
        #~ print("H_t=")
        #~ print(H_t)

        # line 17
        K_t = self.sigma * H_t.T * np.linalg.inv(H_t * self.sigma * H_t.T +
                                                 self.R)

        # line 18
        self.mu += K_t * (z - z_hat)

        print("mu=")
        print(self.mu)

        # line 19
        self.sigma += -K_t * H_t * self.sigma
コード例 #8
0
    def linearize(self, u_1t, z_1t, c_1t, mu_0t):
        '''
        Linearize the constraints (neg log likelihoods) around the given state estimate.
        c.f. p347 ~ p348, Probabilistic Robotics
        === INPUT ===
        u_1t: a list of RobotAction. The k-th item is the action at step k+1.
        z_1t: a list of (a list of Measurement). The k-th item contains the observations at step k+1.
        c_1t: a list of (a list of integers). The k-th item contains the correspondence for the observations at step k+1.
        mu_0t: a (3*T+3+3*N)-by-1 matrix, i.e. the full SLAM state.
              Rows 0:(3*T+3) are robot poses, while rows (3*T+3):(3*T+3+3*N) are map features.
        === OUTPUT ===
        omega: a (3*T+3+3*N)-by-(3*T+3+3*N) matrix, i.e. the informatio matrix.
        ksi: a (3*T+3+3*N)-by-1 matrix, i.e. the information vector.
        '''
        num_states = self.T*3 + 3 + self.N*3

        # line 2
        omega = np.matrix(np.zeros((num_states, num_states))) 
        ksi = np.matrix(np.zeros((num_states, 1))) 

        # line 3. Fix the origin.
        omega[0:3, 0:3] = np.diag([INFINITY, INFINITY, INFINITY])
        rp = RobotPose(0.0, 0.0, 0.0) # change this

        # line 4
        for t in range(1, self.T+1):
            mu_last = self.get_pose(mu_0t, t-1) # robot pose at previous step

            # line 5
            x_t_hat = self.get_pose(mu_0t, t)
            rp_delta = x_t_hat - mu_last

            # line 6
            G_t = np.matrix(np.eye(3))
            G_t[0, 2] = -rp_delta[1, 0]
            G_t[1, 2] =  rp_delta[0, 0]

            # line 7
            tmp = np.matrix(np.zeros((3, 6)))
            tmp[0:3, 0:3] = -G_t
            tmp[0:3, 3:6] = np.eye(3)
            F_t = self.get_motion_update_selection_matrix(t)
            omega += F_t.T * tmp.T * self.R_inv * tmp * F_t # TODO: find a way to do local update

            # line 8
            ksi += F_t.T * tmp.T * self.R_inv * (x_t_hat - G_t * mu_last)

        # line 10
        for t in range(1, self.T+1):
            z_t = self.get_measurements(z_1t, t) # measurements at step t
            c_t = self.get_correspondence(c_1t, t) # correspondences at step t 
            mu_t = self.get_pose(mu_0t, t) # robot pose at step t in matrix form

            # line 12
            for i in range(len(z_t)):
                z_t_i = z_t[i].ToMatrix()

                # line 13
                j = c_t[i]

                # if first seen then add feature to state. NOTE: this part is missing in the book
                if (not self.feature_seen[j]):
                    f = FeatureState(mu_t[0, 0] + z_t_i[0, 0] * np.cos(z_t_i[1, 0] + mu_t[2, 0]), \
                                     mu_t[1, 0] + z_t_i[0, 0] * np.sin(z_t_i[1, 0] + mu_t[2, 0]), \
                                     z_t_i[2, 0])

                    self.set_feature(mu_0t, j, f)
                    self.feature_seen[j] = True

                    print("newly observed " + str(f))

                mu_j = self.get_feature(mu_0t, j) # map feature in matrix form

                # line 14
                delta_x = mu_j[0, 0] - mu_t[0, 0]
                delta_y = mu_j[1, 0] - mu_t[1, 0]

                # line 15
                q = delta_x * delta_x + delta_y * delta_y

                # line 16
                z_t_i_hat = np.matrix([[np.sqrt(q)], \
                                       [wrap_angle(np.arctan2(delta_y, delta_x) - mu_t[2, 0])], \
                                       [z_t_i[2, 0]]])
                # line 17
                tmp = np.matrix(np.zeros((3, 6)))
                tmp[0, 0] = -delta_x * np.sqrt(q)
                tmp[0, 1] = -delta_y * np.sqrt(q)
                tmp[0, 3] =  delta_x * np.sqrt(q)
                tmp[0, 4] =  delta_y * np.sqrt(q)
                tmp[1, 0] =  delta_y 
                tmp[1, 1] = -delta_x 
                tmp[1, 2] = -q
                tmp[1, 3] = -delta_y
                tmp[1, 4] =  delta_x
                tmp[2, 5] =  q
                H_t_i = tmp / q

                # line 18
                F_xj = self.get_measurement_update_selection_matrix(t, j)
                omega += F_xj.T * H_t_i.T * self.Q_inv * H_t_i * F_xj # TODO: find a way to do local update

                # line 19
                tmp = np.matrix(np.zeros((6, 1)))
                tmp[0:3, :] = mu_t
                tmp[3:6, :] = mu_j
                ksi += F_xj.T * H_t_i.T * self.Q_inv * (z_t_i - z_t_i_hat + H_t_i * tmp)

        return omega, ksi
コード例 #9
0
    def linearize(self, u_1t, z_1t, c_1t, mu_0t):
        '''
        Linearize the constraints (neg log likelihoods) around the given state estimate.
        c.f. p347 ~ p348, Probabilistic Robotics
        === INPUT ===
        u_1t: a list of RobotAction. The k-th item is the action at step k+1.
        z_1t: a list of (a list of Measurement). The k-th item contains the observations at step k+1.
        c_1t: a list of (a list of integers). The k-th item contains the correspondence for the observations at step k+1.
        mu_0t: a (3*T+3+3*N)-by-1 matrix, i.e. the full SLAM state.
              Rows 0:(3*T+3) are robot poses, while rows (3*T+3):(3*T+3+3*N) are map features.
        === OUTPUT ===
        omega: a (3*T+3+3*N)-by-(3*T+3+3*N) matrix, i.e. the informatio matrix.
        ksi: a (3*T+3+3*N)-by-1 matrix, i.e. the information vector.
        '''
        num_states = self.T * 3 + 3 + self.N * 3

        # line 2
        omega = np.matrix(np.zeros((num_states, num_states)))
        ksi = np.matrix(np.zeros((num_states, 1)))

        # line 3. Fix the origin.
        omega[0:3, 0:3] = np.diag([INFINITY, INFINITY, INFINITY])
        rp = RobotPose(0.0, 0.0, 0.0)  # change this

        # line 4
        for t in range(1, self.T + 1):
            mu_last = self.get_pose(mu_0t,
                                    t - 1)  # robot pose at previous step

            # line 5
            x_t_hat = self.get_pose(mu_0t, t)
            rp_delta = x_t_hat - mu_last

            # line 6
            G_t = np.matrix(np.eye(3))
            G_t[0, 2] = -rp_delta[1, 0]
            G_t[1, 2] = rp_delta[0, 0]

            # line 7
            tmp = np.matrix(np.zeros((3, 6)))
            tmp[0:3, 0:3] = -G_t
            tmp[0:3, 3:6] = np.eye(3)
            F_t = self.get_motion_update_selection_matrix(t)
            omega += F_t.T * tmp.T * self.R_inv * tmp * F_t  # TODO: find a way to do local update

            # line 8
            ksi += F_t.T * tmp.T * self.R_inv * (x_t_hat - G_t * mu_last)

        # line 10
        for t in range(1, self.T + 1):
            z_t = self.get_measurements(z_1t, t)  # measurements at step t
            c_t = self.get_correspondence(c_1t, t)  # correspondences at step t
            mu_t = self.get_pose(mu_0t,
                                 t)  # robot pose at step t in matrix form

            # line 12
            for i in range(len(z_t)):
                z_t_i = z_t[i].ToMatrix()

                # line 13
                j = c_t[i]

                # if first seen then add feature to state. NOTE: this part is missing in the book
                if (not self.feature_seen[j]):
                    f = FeatureState(mu_t[0, 0] + z_t_i[0, 0] * np.cos(z_t_i[1, 0] + mu_t[2, 0]), \
                                     mu_t[1, 0] + z_t_i[0, 0] * np.sin(z_t_i[1, 0] + mu_t[2, 0]), \
                                     z_t_i[2, 0])

                    self.set_feature(mu_0t, j, f)
                    self.feature_seen[j] = True

                    print("newly observed " + str(f))

                mu_j = self.get_feature(mu_0t, j)  # map feature in matrix form

                # line 14
                delta_x = mu_j[0, 0] - mu_t[0, 0]
                delta_y = mu_j[1, 0] - mu_t[1, 0]

                # line 15
                q = delta_x * delta_x + delta_y * delta_y

                # line 16
                z_t_i_hat = np.matrix([[np.sqrt(q)], \
                                       [wrap_angle(np.arctan2(delta_y, delta_x) - mu_t[2, 0])], \
                                       [z_t_i[2, 0]]])
                # line 17
                tmp = np.matrix(np.zeros((3, 6)))
                tmp[0, 0] = -delta_x * np.sqrt(q)
                tmp[0, 1] = -delta_y * np.sqrt(q)
                tmp[0, 3] = delta_x * np.sqrt(q)
                tmp[0, 4] = delta_y * np.sqrt(q)
                tmp[1, 0] = delta_y
                tmp[1, 1] = -delta_x
                tmp[1, 2] = -q
                tmp[1, 3] = -delta_y
                tmp[1, 4] = delta_x
                tmp[2, 5] = q
                H_t_i = tmp / q

                # line 18
                F_xj = self.get_measurement_update_selection_matrix(t, j)
                omega += F_xj.T * H_t_i.T * self.Q_inv * H_t_i * F_xj  # TODO: find a way to do local update

                # line 19
                tmp = np.matrix(np.zeros((6, 1)))
                tmp[0:3, :] = mu_t
                tmp[3:6, :] = mu_j
                ksi += F_xj.T * H_t_i.T * self.Q_inv * (z_t_i - z_t_i_hat +
                                                        H_t_i * tmp)

        return omega, ksi
コード例 #10
0
    def measurement_update(self, measurement):
        """
        update the state and covar estimate using the measurement
        c.f page 314, Probabilistic Robotics
        """
        print("<---- measurement_update ---->")
        print(measurement)

        rp = self.get_pose()

        # convert measurement into a matrix
        z = measurement.ToMatrix()

        # line 8
        j = measurement.s  # assuming the signature gives the correspondence
        print("correspondence = " + str(j))

        # line 9
        if not self.feature_seen[j]:
            f = FeatureState(
                rp.x + measurement.r * np.cos(measurement.phi + rp.theta),
                rp.y + measurement.r * np.sin(measurement.phi + rp.theta),
                measurement.s,
            )
            # line 10, initialize feature state
            self.set_feature(j, f)
            self.feature_seen[j] = True

            print("newly observed " + str(f))

        fs = self.get_feature(j)

        # line 12
        delta_x = fs.x - rp.x
        delta_y = fs.y - rp.y

        # line 13
        q = delta_x * delta_x + delta_y * delta_y
        # ~ print("delta_x = " + str(delta_x) + " delta_y = " + str(delta_y) + " q= " + str(q))

        # line 14, predicted measurement
        z_hat = np.matrix([[np.sqrt(q)], [wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], [fs.s]])
        # ~ print("z_hat = ")
        # ~ print(z_hat)

        # line 15
        F_xj = self.get_feature_selection_matrix(j)

        # line 16
        h_t_i = np.matrix(np.zeros((3, 6)))
        h_t_i[0, 0] = -delta_x * np.sqrt(q)
        h_t_i[0, 1] = -delta_y * np.sqrt(q)
        h_t_i[0, 3] = delta_x * np.sqrt(q)
        h_t_i[0, 4] = delta_y * np.sqrt(q)
        h_t_i[1, 0] = delta_y
        h_t_i[1, 1] = -delta_x
        h_t_i[1, 2] = -q
        h_t_i[1, 3] = -delta_y
        h_t_i[1, 4] = delta_x
        h_t_i[2, 5] = q
        H_t = h_t_i * F_xj / q
        # ~ print("H_t=")
        # ~ print(H_t)

        # line 17
        K_t = self.sigma * H_t.T * np.linalg.inv(H_t * self.sigma * H_t.T + self.R)

        # line 18
        self.mu += K_t * (z - z_hat)

        print("mu=")
        print(self.mu)

        # line 19
        self.sigma += -K_t * H_t * self.sigma