Ejemplo n.º 1
0
    def PID_controler_with_measured_values(self,k_p,k_d,k_i,mass_err,sigma,use_measured_height=False):
    

        # creating the co-axial drone object 
        Controlled_Drone=CoaxialCopter()
        
        # array for recording the state history 
        drone_state_history = Controlled_Drone.X

        # introducing a small error of the actual mass and the mass for which the path has been calculated
        actual_mass = Controlled_Drone.m * mass_err 

        # creating the control system object 
        control_system = PIDController_with_ff(k_p,k_d,k_i)

        # declaring the initial state of the drone with zero hight and zero velocity 
        Controlled_Drone.X = np.array([0.0,0.0,0.0,0.0])
        
        Drone_IMU = self.IMU(Controlled_Drone.X[0], 0.95)
        observation_hostory = Controlled_Drone.X[0]

        # executing the flight
        for i in range(1,self.z_path.shape[0]-1):
            
            # condition to use height observation to control the drone or
            # use the majically given true state 
            if use_measured_height:
                
                z_observation= Drone_IMU.measure(Controlled_Drone.X[0],sigma)
                
                u_bar = control_system.control(self.z_path[i],
                                               z_observation,
                                               self.z_dot_path[i],
                                               Controlled_Drone.X[2],
                                               self.z_dot_dot_path[i],
                                               self.dt)
                
                observation_hostory = np.vstack((observation_hostory,z_observation))
                
            else:
                
                u_bar = control_system.control(self.z_path[i],
                                               Controlled_Drone.X[0],
                                               self.z_dot_path[i],
                                               Controlled_Drone.X[2],
                                               self.z_dot_dot_path[i],
                                               self.dt)
                
                observation_hostory = np.vstack((observation_hostory,self.z_path[i]))
                
                
            Controlled_Drone.set_rotors_angular_velocities(u_bar,0.0)

            # calculating the new state vector 
            drone_state = Controlled_Drone.advance_state(self.dt, actual_mass)
            
            # generating a history of vertical positions for the drone
            drone_state_history = np.vstack((drone_state_history, drone_state))
            

        
        plt.subplot(211)
        plt.plot(self.t,self.z_path,linestyle='-',marker='.',color='red')
        plt.plot(self.t[1:],drone_state_history[:,0],linestyle='-',color='blue',linewidth=3)
        if use_measured_height:
            plt.scatter(self.t[1:],observation_hostory[:,0],color='black',marker='.',alpha=0.3)
                


        plt.grid()
        if use_measured_height:
            plt.title('Change in height (using measured value)').set_fontsize(20)
        else:
            plt.title('Change in height (ideal case)').set_fontsize(20)
                
        plt.xlabel('$t$ [sec]').set_fontsize(20)
        plt.ylabel('$z-z_0$ [$m$]').set_fontsize(20)
        plt.xticks(fontsize = 14)
        plt.yticks(fontsize = 14)
        if use_measured_height:
            plt.legend(['Planned path','Executed path','Observed value'],fontsize = 14)
        else:
            plt.legend(['Planned path','Executed path'],fontsize = 14)
        plt.show()


        plt.subplot(212)
        plt.plot(self.t[1:],abs(self.z_path[1:]-drone_state_history[:,0]),linestyle='-',marker='.',color='blue')
        plt.grid()
        plt.title('Error value ').set_fontsize(20)
        plt.xlabel('$t$ [sec]').set_fontsize(20)
        plt.ylabel('||$z_{target} - z_{actual}$|| [$m$]').set_fontsize(20)
        plt.xticks(fontsize = 14)
        plt.yticks(fontsize = 14)
        plt.legend(['Error'],fontsize = 14)
        plt.show()
Ejemplo n.º 2
0
    def PID_controler_with_KF(self, position_sigma, motion_sigma, use_kf=False):
    
        # Controller parameters 
        k_p = 20
        k_d = 5 
        k_i = 5 
        mass_err = 1.0 

        
        velocity_sigma = position_sigma           # velocity uncertainty
        
        # creating the co-axial drone object 
        Controlled_Drone=CoaxialCopter()
        
        # array for recording the state history 
        drone_state_history = Controlled_Drone.X

        # introducing a small error of the actual mass and the mass for which the path has been calculated
        actual_mass = Controlled_Drone.m * mass_err 

        # creating the control system object 
        control_system = PIDController_with_ff(k_p,k_d,k_i)

        # declaring the initial state of the drone with zero hight and zero velocity 
        Controlled_Drone.X = np.array([0.0,0.0,0.0,0.0])
        
        Drone_IMU = self.IMU()

        observation_hostory = Controlled_Drone.X[0]

        mu = np.array([[Controlled_Drone.X[1]],[Controlled_Drone.X[0]]]) 
        sigma_cov = np.matmul(np.identity(2), np.array([velocity_sigma,position_sigma]))

        EKFfilter=self.KF(motion_sigma,velocity_sigma,position_sigma,self.dt)
        EKFfilter.initial_values(mu, sigma_cov)
        EKF_history = mu[1,0]

        sigma_cov_history = sigma_cov[1]

        
        # executing the flight
        for i in range(1,self.z_path.shape[0]-1):
            
            # condition to use height observation to control the drone or
            # use the magically given true state 

            z_observation= Drone_IMU.measure(Controlled_Drone.X[0],position_sigma)

            if use_kf:

                z_EKF=mu[1,0]
                v_EKF=mu[0,0]   
                    
                
                
                u_bar = control_system.control(self.z_path[i],
                                               z_EKF,
                                               self.z_dot_path[i],
                                               v_EKF,
                                               self.z_dot_dot_path[i],
                                               self.dt)
                
                
            else:
                
                

                u_bar = control_system.control(self.z_path[i],
                                               z_observation,
                                               self.z_dot_path[i],
                                               Controlled_Drone.X[2],
                                               self.z_dot_dot_path[i],
                                               self.dt)
                


            observation_hostory = np.vstack((observation_hostory,z_observation))

            u_bar=u_bar + np.random.normal(0.0, motion_sigma)
                

            Controlled_Drone.set_rotors_angular_velocities(u_bar,0.0)

            # calculating the new state vector 
            drone_state = Controlled_Drone.advance_state(self.dt, actual_mass)
            
            # generating a history of vertical positions for the drone
            drone_state_history = np.vstack((drone_state_history, drone_state))


            #################
            mu_bar, sigma_bar = EKFfilter.predict(u_bar)
            
            
            z_observation= Drone_IMU.measure(drone_state[0],position_sigma)

            mu, sigma_cov = EKFfilter.update(z_observation)
            #################

            # generating a history of vertical positions for the drone
            EKF_history= np.vstack((EKF_history,mu_bar[1,0]))
            sigma_cov_history = np.vstack((sigma_cov_history,sigma_cov[1,1]))

            

        
        plt.subplot(211)
        plt.plot(self.t,self.z_path,linestyle='-',marker='.',color='red',label = 'Planned path')
        
        if use_kf:
            plt.plot(self.t[1:],EKF_history[:,0],linestyle='-',color='green',linewidth=3,label = 'Estimated path')

        plt.plot(self.t[1:],drone_state_history[:,0],linestyle='-',color='blue',linewidth=3, label = 'Executed path')
        plt.scatter(self.t[1:],observation_hostory[:,0],color='black',marker='.',alpha=0.3, label = 'Observed value')
                


        plt.grid()
        if use_kf:
            plt.title('Change in height (using estimated value)').set_fontsize(20)
        else:
            plt.title('Change in height (using measured value)').set_fontsize(20)
                
        plt.xlabel('$t$ [sec]').set_fontsize(20)
        plt.ylabel('$z-z_0$ [$m$]').set_fontsize(20)
        plt.xticks(fontsize = 14)
        plt.yticks(fontsize = 14)
        plt.legend(fontsize = 14)
        plt.show()


        plt.subplot(212)
        plt.plot(self.t[1:],abs(self.z_path[1:]-drone_state_history[:,0]),linestyle='-',marker='.',color='blue')
        plt.grid()
        plt.title('Error value ').set_fontsize(20)
        plt.xlabel('$t$ [sec]').set_fontsize(20)
        plt.ylabel('||$z_{target} - z_{actual}$|| [$m$]').set_fontsize(20)
        plt.xticks(fontsize = 14)
        plt.yticks(fontsize = 14)
        plt.legend(['Error'],fontsize = 14)
        plt.show()