Problem: Quadrotor Trajectory Tracking Simulation

The problem is to implement quadrotor trajectory tracking simulation in MATLAB with control, kinematics, visualization, and a simulation script.


Input/Output: The problem takes an initial 12×1 state vector s (zeros), a parameter structure para (with b, m, g, Ix, Iy, Iz, l, d, Jr, k1–k6, omegaMax), time parameters (dt=0.01, stime=50), random seed (42), control gains (k1=1, k2=10), leader’s initial position xl ([0;0;0]) and velocity vl ([0;0;0]), and speed parameter sp=1. It outputs a 3D trajectory visualization (via plotHis3), an omega history plot, a state history array xyHis (3×2×5001), and an omega history array omegaHis (4×5000) through simulating quadrotor trajectory tracking with leader state updates, controller, and kinematics functions.

Required dependencies:

Sub-steps:
step_number: 1
step_description_prompt: Please write a MATLAB function named saturate that implements a saturation function using the hyperbolic tangent function. The function takes an input value (or array) and returns the saturated value (or array) via the tanh operation.
Function Definition: function u = saturate(input)
Algorithm Flow: 1. Take the input (a scalar or array) as the function argument. 2. Compute the hyperbolic tangent of the input using the built-in tanh function. 3. Assign the result of tanh(input) to the output variable u. 4. Return u as the function output.
function_header: function u=saturate(input)

end
test_cases: clear; clc;
rng(42)

inputs = [-2, 0, 1, 3];
val = sum(saturate(inputs));
correct = 0.792621;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end

inputs = [3, 1, 2, 6];
val = sum(saturate(inputs));
correct = 3.720664;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end

inputs = [-1, 5, 2, 6];
val = sum(saturate(inputs));
correct = 2.202330;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end
disp('Completely right!')

step_number: 2
step_description_prompt: Please write a MATLAB function named angleDelta that calculates the signed angle difference between two angles p1 and p2. The function is defined as function ang = angleDelta(p2, p1). Algorithm steps: 1. Create two column vectors: vec1 = [cos(p1); sin(p1)] and vec2 = [cos(p2); sin(p2)]. 2. Form a matrix by horizontally concatenating vec1 and vec2 (i.e., [vec1, vec2]), then compute the determinant of this matrix using the det function. 3. Calculate the sign of the determinant result using the sign function. 4. Compute the dot product of vec1 and vec2 using the dot function, then apply the acos function to this dot product to get the arccosine value. 5. Multiply the sign value from step 3 by the acos value from step 4, and assign the product to the output variable ang. Functions such as det, sign, dot, cos, sin, and acos are assumed to be pre-defined and available.
function_header: function ang=angleDelta(p2,p1)

end
test_cases: clear; clc;
rng(42)

p1 = 0;
p2 = -pi/2;
val = angleDelta(p2, p1);
correct = -1.570796;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end

p1 = 0;
p2 = pi;
val = angleDelta(p2, p1);
correct = 3.141593;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end

p1 = pi/4;
p2 = pi/2;
val = angleDelta(p2, p1);
correct = 0.785398;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end
disp('Completely right!')

step_number: 3
step_description_prompt: Please write a MATLAB function named quadrotor_controller that computes the rotor angular velocities for a quadrotor based on the current state, desired position, desired linear velocity, desired yaw angle, system parameters, and control gains.
The function has the definition: function omega = quadrotor_controller(s, xl, vl, psil, para, k1, k2), where the input 's' is the 12x1 current state vector [x; y; z; vx; vy; vz; phi; theta; psi; vphi; vtheta; vpsi], 'xl' is the 3x1 desired position vector, 'vl' is the 3x1 desired linear velocity vector, 'psil' is the scalar desired yaw angle, 'para' is a structure containing quadrotor parameters (b, m, g, Iz, Ix, Iy, l, d), 'k1' and 'k2' are 3x3 control gain matrices, and the output 'omega' is the 4x1 rotor angular velocities vector.
The algorithm flow is as follows:
1. Extract real values from 's' and decompose into state variables: x = s(1), y = s(2), z = s(3), vx = s(4), vy = s(5), vz = s(6), phi = s(7), theta = s(8), psi = s(9), vphi = s(10), vtheta = s(11), vpsi = s(12).
2. Initialize a 4x1 zero vector 'u'.
3. Construct the matrix 'tr' using parameters from 'para': tr = [para.b para.b para.b para.b; 0 -para.b 0 para.b; -para.b 0 para.b 0; -para.d para.d -para.d para.d].
4. Compute 'input_delta' as k1*(xl - [x; y; z]) + k2*(vl - [vx; vy; vz]).
5. Compute 'attr' as saturate(input_delta)*2 + [0; 0; para.g], then multiply 'attr' by the rotation matrix 'rot' where rot = [cos(psi) sin(psi) 0; -sin(psi) cos(psi) 0; 0 0 1].
6. Calculate u(1) as para.m * norm(attr).
7. Compute desired roll angle 'phi_p' as asin(-attr(2)/norm(attr)).
8. Compute desired pitch angle 'theta_p' as atan(attr(1)/attr(3)); if attr(3) < 0, update theta_p as theta_p - pi.
9. Set desired yaw angle 'psi_p' to psil.
10. Calculate u(4) as para.Iz * saturate(angleDelta(psi_p, psi) + 0 - vpsi).
11. Calculate u(2) as 20 * para.Ix * (u(1) + u(4))/2 * saturate(angleDelta(phi_p, phi) + 0 - 1*vphi) / para.l.
12. Calculate u(3) as 20 * para.Iy * (u(1) - u(4))/2 * saturate(angleDelta(theta_p, theta) + 0 - 1*vtheta) / para.l.
13. Compute 'omega2' by solving tr * omega2 = u using the backslash operator.
14. Set all negative elements in 'omega2' to 0.
15. Compute 'omega' as the element-wise square root of 'omega2'.
The functions 'saturate' and 'angleDelta' are assumed to be pre-defined and do not need to be reimplemented.
function_header: function omega=quadrotor_controller(s,xl,vl,psil,para,k1,k2)

end
test_cases: clear; clc;
rng(42);
para.b = 1.0;
para.d = 0.1;
para.m = 1.0;
para.g = 9.81;
para.Ix = 0.1;
para.Iy = 0.1;
para.Iz = 0.2;
para.l = 0.5;

k1 = 1.0;
k2 = 1.0;

s1 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]';
xl1 = [0, 0, 0]';
vl1 = [0, 0, 0]';
psil1 = 0;
omega1 = quadrotor_controller(s1, xl1, vl1, psil1, para, k1, k2);
val = sum(omega1);
correct = 6.264184;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end

s2 = [1, 2, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0]';
xl2 = [0, 0, 0]';
vl2 = [0, 0, 0]';
psil2 = 0;
omega2 = quadrotor_controller(s2, xl2, vl2, psil2, para, k1, k2);
val = sum(omega2);
correct = 4.962581;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end

s3 = [0, 0, 0, 0, 0, 0, 0.1, 0.2, 0.3, 0, 0, 0]';
xl3 = [1, 2, 3]';
vl3 = [0.1, 0.2, 0.3]';
psil3 = 0.5;
omega3 = quadrotor_controller(s3, xl3, vl3, psil3, para, k1, k2);
val = sum(omega3);
correct = 6.536922;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end
disp('Completely right!')

step_number: 4
step_description_prompt: Please write a MATLAB function named quadrotor_kinematics that implements the quadrotor kinematics update based on the current state vector, four rotor angular velocities, system parameters, and time step, returning the updated state vector.
function s = quadrotor_kinematics(s, omega, para, dt)
First, unpack the input state vector s into scalar variables: x = s(1), y = s(2), z = s(3), vx = s(4), vy = s(5), vz = s(6), phi = s(7), theta = s(8), psi = s(9), vphi = s(10), vtheta = s(11), vpsi = s(12).
Define the matrix tr using parameters para.b and para.d as [para.b para.b para.b para.b; 0 -para.b 0 para.b; -para.b 0 para.b 0; -para.d para.d -para.d para.d].
Calculate omega2 as the element-wise square of the omega vector.
Calculate Omega as -omega(1) + omega(2) - omega(3) + omega(4).
Compute the control input vector u as the product of tr and omega2 (matrix multiplication).
Update the linear velocity vx and position x using: vx = vx + dt * (u(1)*(cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi))/para.m - para.k1*vx/para.m); x = x + dt*vx.
Update vy and y using: vy = vy + dt * (u(1)*(sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi))/para.m - para.k2*vy/para.m); y = y + dt*vy.
Update vz and z using: vz = vz + dt * (u(1)*(cos(theta)*cos(phi))/para.m - para.g - para.k3*vz/para.m); z = z + dt*vz.
Update the angular velocity vphi and angle phi using: vphi = vphi + dt * ((para.Iy - para.Iz)/para.Ix * vtheta*vpsi - para.Jr*vtheta*Omega + u(2)*para.l/para.Ix - para.k4*vphi*para.l/para.Ix); phi = phi + dt*vphi.
Update vtheta and theta using: vtheta = vtheta + dt * ((para.Iz - para.Ix)/para.Iy * vphi*vpsi + para.Jr*vphi*Omega + u(3)*para.l/para.Iy - para.k5*vtheta*para.l/para.Iy); theta = theta + dt*vtheta.
Update vpsi and psi using: vpsi = vpsi + dt * ((para.Ix - para.Iy)/para.Iz * vphi*vtheta + u(4)/para.Iz - para.k6*vpsi/para.Iz); psi = psi + dt*vpsi.
Pack the updated scalar variables back into the state vector s in the order [x; y; z; vx; vy; vz; phi; theta; psi; vphi; vtheta; vpsi].
Return the updated state vector s.
Assume all parameters in the para structure (including b, d, m, g, Ix, Iy, Iz, Jr, l, k1, k2, k3, k4, k5, k6) are pre-defined and valid.
function_header: function s=quadrotor_kinematics(s,omega,para,dt)

end
test_cases: clear; clc;
rng(42);

para.b = 1.0;
para.d = 0.1;
para.m = 1.0;
para.g = 9.81;
para.Ix = 0.1;
para.Iy = 0.1;
para.Iz = 0.2;
para.l = 0.5;
para.Jr = 0.01;
para.k1 = 0.1;
para.k2 = 0.1;
para.k3 = 0.1;
para.k4 = 0.1;
para.k5 = 0.1;
para.k6 = 0.1;
dt = 0.01;

s1 = [1.7, 2.4, 3.6, 2, 1, 0, 0.5, 0, 0.2, 0, 1.7, 0]';
omega1 = [sqrt(para.m*para.g/(4*para.b)), sqrt(para.m*para.g/(4*para.b)), sqrt(para.m*para.g/(4*para.b)), sqrt(para.m*para.g/(4*para.b))]';
s1_new = quadrotor_kinematics(s1, omega1, para, dt);
val = sum(s1_new);
correct = 13.086138;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end

s2 = [0, 0, 1.2, 0, 0, 0, 0.1, 0.2, 0.3, 0.1, 0.2, 0.3]';
omega2 = [1, 1.1, 1.2, 1.3]';
s2_new = quadrotor_kinematics(s2, omega2, para, dt);
val = sum(s2_new);
correct = 2.414815;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end

s3 = [0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 0]';
omega3 = [2, 2, 2, 2]';
s3_new = quadrotor_kinematics(s3, omega3, para, dt);
val = sum(s3_new);
correct = 6.116459;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end
disp('Completely right!')

step_number: 5
step_description_prompt: Please use MATLAB code to create a function named plotHis3 that visualizes the 3D trajectory history of multiple agents over time with adjustable tail length, animation speed, and stop time.
Function definition: function plotHis3(xHis,dt,tailLength,speed,tt)
1. Handle input arguments based on nargin: if nargin==2, set tailLength=-1, speed=-1, tt=-1; elseif nargin==3, set speed=-1, tt=-1; elseif nargin==4, set tt=-1; end.
2. Get the number of time steps (loop) from xHis: [~,~,loop] = size(xHis); compute total simulation time ts = dt*(loop-1).
3. Adjust tailLength to a valid range: if tailLength>loop || tailLength<0, set tailLength=loop; adjust speed similarly: if speed>loop || speed<0, set speed=loop.
4. Downsample xHis to xyHis based on speed: initialize xyHis as an empty matrix; compute skip=round(loop/speed); if skip>1, loop k from 1 to loop with step skip, append xHis(:,:,k) to xyHis (set xyHis=xHis(:,:,1) when k==1, else xyHis(:,:,end+1)=xHis(:,:,k)); if the last k is not equal to loop, append xHis(:,:,end) to xyHis; if skip<=1, set xyHis=xHis.
5. Get the number of agents (n) and new time steps (loop) from xyHis: [~,n,loop] = size(xyHis).
6. Compute axis limits with 5% padding:
   a. amin = min(min(min(xyHis(1,:,:)))); amax = max(max(max(xyHis(1,:,:))));
   b. bmin = min(min(min(xyHis(2,:,:)))); bmax = max(max(max(xyHis(2,:,:))));
   c. cmin = min(min(min(xyHis(3,:,:)))); cmax = max(max(max(xyHis(3,:,:))));
   d. aw = amax - amin; if aw==0, set aw=1; bw = bmax - bmin; if bw==0, set bw=1;
   e. amin = amin - 0.05*aw; amax = amax + 0.05*aw; bmin = bmin - 0.05*bw; bmax = bmax + 0.05*bw.
7. Reshape xyHis to a 2D matrix xyz for tail plotting: xyz = reshape(xyHis, 3*n, loop).
8. Initialize a waitbar with the message 'In progress>>>>>>>>>>': hwait = waitbar(0,'In progress>>>>>>>>>>').
9. Loop over each time step k from 1 to loop:
   a. Plot initial positions of all agents as black 'x' markers: plot3(xyHis(1,:,1),xyHis(2,:,1),xyHis(3,:,1),'kx').
   b. Enable hold for multiple plots: hold on.
   c. Plot the current position of the first agent as a red 'p' marker: plot3(xyHis(1,1,k),xyHis(2,1,k),xyHis(3,1,k),'rp').
   d. Plot current positions of other agents as blue 'o' markers with blue fill color and size 5: plot3(xyHis(1,2:end,k),xyHis(2,2:end,k),xyHis(3,2:end,k),'bo', 'MarkerFaceColor','b','MarkerSize',5).
   e. Plot the main trajectory tail: set tail=tailLength; if k<=tailLength || tailLength<0, plot3(xyz(1,1:k),xyz(2,1:k),xyz(3,1:k),'m--'); else plot3(xyz(1,k-tail:k),xyz(2,k-tail:k),xyz(3,k-tail:k),'m--').
   f. Plot trajectory tails for each agent from the 2nd to the nth: for i=2:n, if k<=tailLength || tailLength<0, plot3(xyz(3*i-2,1:k),xyz(3*i-1,1:k),xyz(3*i,1:k),'c-'); else plot3(xyz(3*i-2,k-tail:k),xyz(3*i-1,k-tail:k),xyz(3*i,k-tail:k),'c-'); end.
   g. Disable hold: hold off.
   h. Update the plot title with the current time: title(['time=',num2str(k/loop*ts)]).
   i. Set axis limits: axis([amin amax bmin bmax cmin cmax]).
   j. Check if the current time exceeds tt (if tt>0): if tt>0 && (k/loop*ts>=tt), break the loop.
   k. Update the waitbar with progress and message 'Optimizing in progress': waitbar(k/loop,hwait,'Optimizing in progress').
10. Close the waitbar after the loop: close(hwait).
11. Add axis labels: xlabel('x'); ylabel('y'); zlabel('z').
Assume all called functions (waitbar, plot3, hold, title, axis, num2str, reshape, min, max, round, size, close, xlabel, ylabel, zlabel) are already defined and available.
ground_truth_code: function plotHis3(xHis,dt,tailLength,speed,tt)
if nargin==2
    tailLength=-1;
    speed=-1;
    tt=-1;
elseif nargin==3
    speed=-1;
    tt=-1;
elseif nargin==4
    tt=-1;
end
[~,~,loop]=size(xHis);
ts=dt*(loop-1);
if tailLength>loop || tailLength<0
    tailLength=loop;
end
if speed>loop || speed<0
    speed=loop;
end
xyHis=[];
skip=round(loop/speed);
if skip>1
    for k=1:skip:loop
        if k==1
            xyHis=xHis(:,:,1);
        else
            xyHis(:,:,end+1)=xHis(:,:,k);
        end
    end
    if k~=loop
        xyHis(:,:,end+1)=xHis(:,:,end);
    end
else
    xyHis=xHis;
end
[~,n,loop]=size(xyHis);
amin=min(min(min(xyHis(1,:,:))));
amax=max(max(max(xyHis(1,:,:))));
bmin=min(min(min(xyHis(2,:,:))));
bmax=max(max(max(xyHis(2,:,:))));
cmin=min(min(min(xyHis(3,:,:))));
cmax=max(max(max(xyHis(3,:,:))));
aw=amax-amin;
if aw==0
    aw=1;
end
bw=bmax-bmin;
if bw==0
    bw=1;
end
amin=amin-0.05*aw;
amax=amax+0.05*aw;
bmin=bmin-0.05*bw;
bmax=bmax+0.05*bw;
xyz=reshape(xyHis,3*n,loop);
hwait=waitbar(0,'进行中>>>>>>>>>>');
for k=1:loop
    plot3(xyHis(1,:,1),xyHis(2,:,1),xyHis(3,:,1),'kx')
    hold on
    plot3(xyHis(1,1,k),xyHis(2,1,k),xyHis(3,1,k),'rp')
    plot3(xyHis(1,2:end,k),xyHis(2,2:end,k),xyHis(3,2:end,k),'bo', 'MarkerFaceColor','b','MarkerSize',5)

    tail=tailLength;
    if k<=tailLength || tailLength<0
        plot3(xyz(1,1:k),xyz(2,1:k),xyz(3,1:k),'m--')
    else
        plot3(xyz(1,k-tail:k),xyz(2,k-tail:k),xyz(3,k-tail:k),'m--')
    end
    for i=2:n
        if k<=tailLength || tailLength<0
            plot3(xyz(3*i-2,1:k),xyz(3*i-1,1:k),xyz(3*i,1:k),'c-')
        else
            plot3(xyz(3*i-2,k-tail:k),xyz(3*i-1,k-tail:k),xyz(3*i,k-tail:k),'c-')
        end
    end
    hold off
    title(['time=',num2str(k/loop*ts)]);

    axis([amin amax bmin bmax cmin cmax]);
    if tt>0
        if k/loop*ts>=tt
            break
        end
    end

    waitbar(k/loop,hwait,'优化进行中');
end
close(hwait);
xlabel('x')
ylabel('y')
zlabel('z')

end
function_header: function plotHis3(xHis,dt,tailLength,speed,tt)

end

step_number: 6
step_description_prompt: Please use MATLAB code to create a script that implements quadrotor trajectory tracking simulation with specified initial conditions, parameter configurations, control logic, and result visualization as follows:

1. Clear the workspace and command window:  
   ```matlab
   clear all; clc;
   ```

2. Set the random seed for reproducibility:  
   ```matlab
   rng(42);
   ```

3. Define time parameters:  
   ```matlab
   dt = 0.01;
   stime = 50;
   loop = stime / dt;
   ```

4. Define dimensions:  
   ```matlab
   d = 3;
   n = 1;
   ```

5. Initialize the 12×1 state vector `s` (all initial values are zero due to `unifrnd` bounds):  
   ```matlab
   s = zeros(12, n);
   s(1:3, :) = unifrnd(-0, 0, [3, n]);
   s(4:6, :) = unifrnd(-0, 0, [3, n]);
   s(7, :) = unifrnd(-0.0*pi, 0.0*pi, [1, n]);
   s(8, :) = unifrnd(-0.0*pi, 0.0*pi, [1, n]);
   s(9, :) = unifrnd(-0.0*pi, 0.0*pi, [1, n]);
   ```

6. Extract individual state variables from `s` (redundant but retained for consistency):  
   ```matlab
   x = s(1); y = s(2); z = s(3);
   vx = s(4); vy = s(5); vz = s(6);
   phi = s(7); theta = s(8); psi = s(9);
   vphi = s(10); vtheta = s(11); vpsi = s(12);
   ```

7. Initialize the leader’s position (`xl`) and velocity (`vl`):  
   ```matlab
   xl = [0; 0; 0];
   vl = [0; 0; 0];
   ```

8. Define the parameter structure `para` with exact values:  
   ```matlab
   para.g = 9.8;
   para.m = 1.2;
   para.Iy = 0.05;
   para.Ix = 0.05;
   para.Iz = 0.1;
   para.b = 1e-4;
   para.l = 0.5;
   para.d = 1e-6;
   para.Jr = 0.01;
   para.k1 = 0.02;
   para.k2 = 0.02;
   para.k3 = 0.02;
   para.k4 = 0.1;
   para.k5 = 0.1;
   para.k6 = 0.1;
   para.omegaMax = 330;
   ```

9. Initialize the state history array `xyHis` (size: 3×2×5001) and set initial states:  
   ```matlab
   xyHis = zeros(d, n+1, loop+1);
   xyHis(:, :, 1) = [xl s(1:3)];
   ```

10. Create a waitbar with the initial message `'½øÐÐÖÐ>>>>>>>>>>'`:  
    ```matlab
    hwait = waitbar(0, '½øÐÐÖÐ>>>>>>>>>>');
    ```

11. Define the speed parameter:  
    ```matlab
    sp = 1;
    ```

12. Initialize the omega history array (size: 4×5000):  
    ```matlab
    omegaHis = zeros(4, loop);
    ```

13. Run the main simulation loop (5000 iterations):  
    ```matlab
    for t = 1:loop
        % Calculate leader acceleration based on time ratio
        if t/loop < 0.1
            al = ([0; 0; sp] - vl);
        elseif t/loop < 0.2
            al = ([sp; 0; 0] - vl);
        elseif t/loop < 0.4
            al = ([0; sp; 0] - vl);
        elseif t/loop < 0.6
            al = ([-sp; 0; 0] - vl);
        elseif t/loop < 0.8
            al = ([0; -sp; 0] - vl);
        elseif t/loop < 0.9
            al = ([sp; 0; sp] - vl);
        else
            al = ([sp; 0; 0] - vl);
        end

        % Update leader state
        vl = vl + dt * al;
        xl = xl + dt * vl;

        % Get motor speeds from controller (pre-defined function)
        omega = quadrotor_controller(s, xl, vl, 0, para, 1, 10);
        omegaHis(:, t) = omega;

        % Update quadrotor state (pre-defined function)
        s = quadrotor_kinematics(s, omega, para, dt);

        % Store current states
        xyHis(:, :, t+1) = [xl s(1:3)];

        % Update waitbar
        waitbar(t/loop, hwait, 'simulating...');
    end
    ```

14. Close the waitbar:  
    ```matlab
    close(hwait);
    ```

15. Visualize results:  
    - 3D trajectory using the pre-defined `plotHis3` function:  
      ```matlab
      figure(1);
      plotHis3(xyHis, dt, -1, 200);
      axis equal;
      grid on;
      ```
    - Omega history:  
      ```matlab
      figure(2);
      plot(omegaHis');
      grid on;
      ```

Note: The functions `quadrotor_controller`, `quadrotor_kinematics`, and `plotHis3` are assumed to be pre-defined and available in the MATLAB path. Ensure all variables, parameters, and logic match exactly to reproduce the original results.
test_cases: rng(42)
val = sum(omega) + sum(s);
correct = 700.622169;
tol = abs(val-correct);
if tol>1e-4
    error('Wrong!')
end
disp('Completely right!')

General solution:

General tests: