コード例 #1
0
ファイル: default_old.py プロジェクト: yantao006/autonavx_web
    def measurement_callback(self, marker_position_world, marker_yaw_world,
                             marker_position_relative, marker_yaw_relative):
        '''
        :param measurement: vector of measured coordinates
        '''

        while (marker_yaw_relative > math.pi):
            marker_yaw_relative -= 2 * math.pi
        while (marker_yaw_relative < -math.pi):
            marker_yaw_relative += 2 * math.pi
        from math import sin, cos
        #meas = Pose2D(marker_yaw_world, marker_position_world) * Pose2D(marker_yaw_relative, marker_position_relative).inv();
        measurement = np.array([[
            marker_position_relative[0], marker_position_relative[1],
            marker_yaw_relative
        ]]).T

        s_psi = sin(self.x[2])
        c_psi = cos(self.x[2])

        #TODO: Enter the Jacobian derived in the previous assignment
        dx = marker_position_world[0] - self.x[0]
        dy = marker_position_world[1] - self.x[1]
        self.H = np.array([[-c_psi, -s_psi, s_psi * dx - c_psi * dy],
                           [s_psi, -c_psi, c_psi * dx + s_psi * dy],
                           [0, 0, -1]])
        I = np.zeros((3, 3))
        self.H = np.hstack((self.H, I))

        predicted_meas = Pose2D(self.x[2], self.x[0:2]).inv() * Pose2D(
            marker_yaw_world, marker_position_world)
        predicted_measurement = np.array([[
            predicted_meas.translation[0], predicted_meas.translation[1],
            predicted_meas.yaw()
        ]]).T

        while (predicted_measurement[2] > math.pi):
            predicted_measurement[2] -= 2 * math.pi
        while (predicted_measurement[2] < -math.pi):
            predicted_measurement[2] += 2 * math.pi

        print "z_predicted", predicted_measurement.T
        print "z", measurement.T

        # visualize measurement
        plot_point("gps", measurement[0:2])

        k = self.calculateKalmanGain(self.sigma, self.H, self.R)
        #print "kalman", k
        #print "before", self.x.T
        self.x = self.correctState(measurement, self.x, k,
                                   predicted_measurement)
        self.sigma = self.correctCovariance(self.sigma, k, self.H)
        #print "after", self.x.T
        # visualize position state
        p = self.x[0:2]
        plot_trajectory("kalman", p)
        plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
コード例 #2
0
    def measurement_callback(self, measurement):
        '''
        param measurement: vector of measured coordinates
        '''

        # Visualize measurement
        plot_point("gps", measurement)
        k = self.calculateKalmanGain(self.sigma, self.H, self.R)
        self.x = self.correctState(measurement, self.x, k, self.H)
        self.sigma = self.correctCovariance(self.sigma, k, self.H)

        # Visualize position state
        plot_trajectory("kalman", self.x[0:2])
        plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
コード例 #3
0
ファイル: hw01.py プロジェクト: z80/hw
 def measurement_callback(self, measurement):
     '''
     :param measurement: vector of measured coordinates
     '''
     
     # visualize measurement
     plot_point("gps", measurement)
     
     k = self.calculateKalmanGain(self.sigma, self.H, self.R)
     
     self.x = self.correctState(measurement, self.x, k, self.H)
     self.sigma = self.correctCovariance(self.sigma, k, self.H)
     
     # visualize position state
     plot_trajectory("kalman", self.x[0:2])
     plot_covariance_2d("kalman", self.sigma[0:2,0:2])