# Quad-rotor 1-D Linear Control Simulation with MATLAB

I was Performing Programming Assignment with MATLAB for Quad Thrust and Height for an Introductory Course. The control input for a PD controller was According to the Equation

u = mass*(diff(s_des, 2) + Kpe + Kvdiff(e) + gravity)

where:

1. diff(s_des, 2) is the second diff. of the Desired Height which is 1 meter.
2. Kp & Kv are the Proportional and Derivative Gains which Require Tuning.
3. The Term "e" is the Position Error and diff(e) is the Velocity Error.

Note: The Desired Kp, Kv, and Height required to be within 1 Second for the Rise Time, and Less than 5% for the Over Shooting.

and the Code made by the instructor was:

function [ u ] = pd_controller(~, s, s_des, params)
% PD_CONTROLLER  PD controller for the height
% s: 2x1 vector containing the current state [z; v_z]
% s_des: 2x1 vector containing desired state [z; v_z]
% params: robot parameters
% FILL IN YOUR CODE HERE
end


Finally, The simulator doesn't go so well, and I Couldn't Find the Error

• The code after % FILL IN YOUR CODE HERE (my Solution) is yours? – fibonatic Aug 8 '18 at 3:35
• usually diff of constant are null, you need to have time variant functions for your desired trajectory – N. Staub Aug 8 '18 at 7:29
• Yes, its mine and I was a little bit confused when I was solving this Cause its my first time, and I don't know how I couldn't Figure Out this Thank you N. Staub – Ammar Taha Aug 8 '18 at 13:45
• The function has s and s_des as inputs, but you always override them. Do you know why is the first input of the function ignored (due to the ~)? Also note that s and s_des is the state vector of the position and its velocity (its first derivative). – fibonatic Aug 8 '18 at 14:41
• @AmmarTaha please keep your own code so we can point out possible issues – N. Staub Aug 10 '18 at 11:49

You haven't posted how you're using your function, which could be problematic, or what the error is. That said, even without that information (which will be required to get a more detailed answer), there are a few glaring issues. I'll point those out now and request that you edit your question to include how you're using the function you provided to perform a simulation, what you were expecting to see in the simulation, and what you actually got, including the exact errors you got, if any.

1. You gave a "control equation" of u = mass*(diff(s_des, 2) + Kpe + Kvdiff(e) + gravity). The term diff(s_des, 2) doesn't make sense in the context of the question because (a) your s_des doesn't have three entries, so you can't take diff(s_des,2) (the result is empty), and (b) you're (trying to) use the reference acceleration as a feed-forward term for your controller. This may or may not be in the scope of what you're trying to do, but your question asked specifically about a PD controller. I don't know if you're supposed to have the feed-forward term included.
2. Big Problem #1 - You get the current height and speed, s, and then you overwrite it: s = [0; diff(0)]; You should try just entering that command at the command prompt in Matlab and see what the result is. It's not a 2x1 vector. It certainly is not the current height and speed.
3. Big Problem #2 - Same as Big Problem #1, but for your references s_des.
4. Your error term e and the derivative error term diff(e) suffer from a similar-ish set of problems as the two Big Problems above. You're not using diff correctly. Instead of trying to take diff(e) to get the time derivative of your error, you should instead consider what constitutes your error (hint:position) and what the derivative of a position error would be, and then how you can create that given the current state and reference states you've been given.

Since most of your trouble seems to revolve around diff and indexing here's a quick primer on diff:

If you have a symbolic expression, then diff will give you the symbolic derivative of that expression. You do not have a symbolic expression. You have a 2x1 numeric vector.

If you have a numeric vector, then diff(myVector) is equivalent to diff(myVector, 1), which is equivalent to myVector(2:end) - myVector(1:end-1). That is, it starts at the second entry in the vector and subtracts from that entry the value in the previous index. It then steps through the entire remainder of the vector. If myVector is Nx1, then diff(myVector) is (N-1)x1.

If you try diff(myVector, M), then Matlab runs the diff(myVector) M times and returns that result. Every time it runs the diff command your result gets shorter by one. This is why I said earlier that you can't take diff(s_des, 2) if s_des is only 2 elements big. The starting vector is 2x1, so the result of the first diff is (2-1)x1, and the result of the second diff is then ((2-1)-1)x1, or 0x1. It's empty.

Further, from a physical meaning perspective, your s and s_des vectors are [position; speed]. What do you get when you take diff(s)? You get [speed - position]. What physical meaning does that have? None.

What I would suggest you do (and what I routinely do with all of my code) is to "break out" your inputs as the first thing you do in your code. Since the first entry in s is your current position, you might try something like:

currentPos = s(1);


You can break out the rest of the terms in a similar manner:

currentSpeed = s(2);
referencePos = s_des(1);
referenceSpeed = s_des(2);


Also important to note is that these terms are not a history of values. That is, you can't take diff(currentPos) and get a speed. currentPos is exactly that - the current position. Nothing more. Again, I would suggest you write the reference error equation and then take the first derivative of that.

• Hi Chuck, I Really do Appreciate Your Desired Effort for Trying to Help me, and I Know that my Problem has many Shits Require Fixing. I am Familiar with mathematics and the Physical Meanings in contrast with Numeric Computations Using MATLAB, so Every thing comes in Conflict. What Matters now is that I do have Feedforward term which don't know how to deal with it. The Simulator Must give me Desired Trajectory of 1 meter Height, <5% Overshooting, and < 1 Second to reach the Height, but it Doesn't Response at all Without Completing the Full Terms of the equation which needs the Feed Term now. – Ammar Taha Aug 8 '18 at 21:21