MATLAB PID loop implementation for drone target tracking

Question

Basically, I am trying to implement a basic PD controller to throttle the angular and linear velocity of a drone tracking a target. Tflite provides me with target bounding box data (x,y), and from that I can get x and y error (in pixels). Based on that error, I wanna give the drones velocity commands (using MAVROS so easiest way is velocity commands).

I am not concerned with the FOV / distortion of the camera and how distance from target affects actual physical distance vs error in pixels right now, that math can be taken care of later.

Below is matlab code simulating basically how it would go, timestep is based on ROS::RATE of 20hz. I got some gains that seem to give realistic output (not instantly speeding the drone up to 1 million rad/s, lol) but the acceleration curve is still reaaally steep, much too steep to have any impact whatsoever. Is there something inherently wrong with my implementation?

formula used: vel (control variable) = Kp * error + Kd * d_error/dt

matlab script output

clear
clc
clf

Kp = 0.008; 
Kd = -0.9;

error = 200; %pixels

iterations = 10000;
timestep = 0.05; %s
total_time = 0; 
vel = 0;

x = zeros(1,10000);
y = zeros(1,10000);
y2 = zeros(1,10000);

for i = 1:iterations
    
    x(i) = total_time;
    y(i) = error;
    y2(i) = vel; % rad/s
    
    prev_error = error; 
    
    error = error - vel*timestep;
    
    d_error = error - prev_error;
    
    vel = Kp * error + Kd * d_error / timestep;
    
    total_time = total_time + timestep;
    
end

hold on

plot(x,y);
plot(x,y2);

Answer 1

From what i understand, your physical system is modelled:

$ \dot{x} = u $

So it can perfectly follow a velocity command. (In reallity, due to the system inertia, it won't follow it perfectly, so even if your velocity command is very steep, the drone will accelerate at a slower pace).

Usually, the commanded quantity (force or torque in a 2 DOF system, and the velocity in your case) is the highest in the start of the command, as the error is the highest.

Also, just to clarify as i am a bit confused (because you mention an acceleration curve while there is none), you are plotting the error vs time (blue line) and the velocity command vs time (orange line). So the only way to deduce the steep acceleration is from the jump in the velocity.

So, the spike at the start is normal and a result of your modelling of the system dynamics.