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.
- 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.
- 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.
- Big Problem #2 - Same as Big Problem #1, but for your references
- 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
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_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.