<jupyter_start><jupyter_text>Feedforward controlThe feedforward term captures the acceleration of the target path and adds to the $\bar{u}_1$ calculated by PD controller. $$\ddot{z} = \bar{u}_1= k_p(z_t-z)+k_d(z_t-z)+\ddot{z}$$   TODO - Implement PD controller WITH feed forward accelerationModify `thrust_control` to incorporate the feedforward term into the PD Controller math.$$\begin{align}e &= z_{\text{target}} - z_{\text{actual}} \\\dot{e} &= \dot{z}_{\text{target}} - \dot{z}_{\text{actual}} \\\bar{u}_1 &= k_p e + k_d \dot{e} + \ddot{z}_{\text{ff}} \\u_1 &= m(g - \bar{u}_1)\end{align}$$<jupyter_code>class PDController:
    
    def __init__(self, k_p, k_d, m):
        self.k_p = k_p
        self.k_d = k_d
        self.vehicle_mass = m
        self.g = 9.81
        
    def tune(self, delta, omega_n):
        self.k_p = omega_n ** 2
        self.k_d = 2 * delta * omega_n
    
    def thrust_control(self,
                z_target, 
                z_actual, 
                z_dot_target, 
                z_dot_actual,
                z_dot_dot_ff=0.0):
        
        err = z_target - z_actual
        err_dot = z_dot_target - z_dot_actual
        u_bar = self.k_p * err + self.k_d * err_dot + z_dot_dot_ff
        u = self.vehicle_mass * (self.g - u_bar) 
        
        return u
    
testing.pd_controller_test(PDController, feed_forward=True)
MASS_ERROR = 1.0

drone = Monorotor()
perceived_mass = drone.m * MASS_ERROR
controller = PDController(5.0, 2.0, perceived_mass)

t, z_path, z_dot_path = trajectories.step()
dt = t[1] - t[0]

history = []
for z_target, z_dot_target in zip(z_path, z_dot_path):
    z_actual = drone.z
    z_dot_actual = drone.z_dot
    u = controller.thrust_control(z_target, z_actual, 
                                  z_dot_target, z_dot_actual)
    drone.thrust = u
    drone.advance_state(dt)
    history.append(drone.X)
    
z_actual = [h[0] for h in history]
plotting.compare_planned_to_actual(z_actual, z_path, t)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.12356178089044523, 'fraction_numerical': 0.010505252626313157, 'mean_word_length': 2.5778175313059033, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '1', 'dir': 'jupyter-structured-clean-dedup', 'id': '240362', 'license': 'MIT', 'n_tokens_mistral': 768, 'n_tokens_neox': 732, 'n_words': 155, 'path': 'jupyter_notebooks/3_Controls/2_IntroductionToVehicleControl/More Model Errors.ipynb', 'repo_name': 'miker2/FCND-udacity', 'size': '1999'}