FloatShield: LQ Control
PeterChmurciak opened this issue · 17 comments
@gergelytakacs @mgulan I am currently trying to implement LQ control on FloatShield R1 unit in Arduino IDE, but am not sure how to go on about it. Within matlab it is straightforward, as matrix operations are its strong point, but it is not so with Arduino IDE.
Of course it is possible to work in Arduino IDE with arrays of arrays (~matrices) or with just arrays by using library for matrix operations. However I assume that these operations with A,B,C,D,X,U matrices will be pretty time consuming.
Could you advise me if there is some better way of implementing LQ on Arduino board or it will be "same" as it would be in matlab?
@PeterChmurciak
Before I advise anything else, I would suggest to look into the last year’s bachelor thesis of Anna Vargová, which can be also found on the AutomationShield server. I believe it was her topic and offers a good review, implementation aspects, etc.
@gergelytakacs @mgulan @erik1392 I am working on getting better results from FloatShield LQ example but there are some issues so I would like to ask for your advice.
My current approach:
I am using identification example to get continuous linear state-space model from measured data. When the model-data fit is satisfactory, by using c2d I get discrete linear state-space model, and then with dlqr I get LQ gain from these discrete matrices. After that I use the matrices with LQ gain in the arduino LQ example and try how it works. Then I repeat this process with different data or different parts of the same data.
For example our default identification data:
After getting discrete matrices of that model, Kalman filtering with Q=diag(ones(1,3)); and R = 1; looks like:
For comparison Kalman filtering with same Q but R = 100; looks like:
With higher R the filtering is more smooth but is shifted - estimates are higher than real values.
Continuing with R=1:
At the top is ball velocity gained by calculating differences between all adjacent ball position measurements and dividing them by Ts.
In the middle is the same but gained from ball position estimated/filtered with R=1.
At the bottom is estimated second state - estimated ball velocity.
As expected, due to the fact that ball position measurement is pretty noisy itself, the velocity calculated from it is even noisier. However surprisingly the velocity estimated by Kalman filter looks pretty reasonable. Closer look at the estimated ball velocity:
Finally the third state - air speed in tube estimated looks like:
Although we do not have any measurements of the air speed, this one looks pretty wrong, however it does not reach any absudly high values so it might not be an issue.
Results from LQ example:
In this example I have manually gradually increased reference value (in mm) and recorded the ball behaviour. As you can see by slow and gradual increases, the regulator is able to stabilise the ball up to the middle of the tube. However for some reason that is as far as it can get - with reference value at maximum - 324mm (100%), the ball is still only in the middle of the tube and does not rise. It is stabilised there, but does not rise.
At the end of the measurement I wanted to depict the behaviour of the ball with sudden power changes - it is very susceptible to oscillation, and when oscillating, it is not able to stabilise back.
In the example the R was equal to 100, but even when set to 0, the ball is not able to go any higher.
(I have used the value 10^2 as the measurement noise is at most 10 mm)
As it is, the ball can be stabilised but only with gradual changes, only up to the half of the tube height and it does not follow the reference value as it should.
Do you have any idea where I should look for the problem ?
Additionaly could you please @mgulan give me your opinion on something ? In the LQ example there are lines:
The actual height is measured by sensor, then state vector is estimated by kalman filter, then LQ regulator system input is calculated, and with that the fan is actuated.
What bothers me is the order of those operations - if we would give the Kalman filter the current y and current u then it would give us estimate of current states X - therefore we should first calculate current LQ input u and take measurement y and after that estimate X so everything is actual. Currently the provided u is not actual, but the one from previous iteration.
The problem I see is that the calculated LQ input u needs to know the state vector X before its calculation - but we do not know the state vector X and that is why we estimate it - both things are dependent on each other. Can you please advise me how to go on about it ? What order of operations would you use ?
One more thing if I may:
In reference vector should be the values we want - the values to which the regulator should control the states. Currently only the first one - the reference altitude is provided. But what about reference ball speed or reference air speed ? The ball, when stabilised, probably has zero velocity so that one might be zero. But what about the air speed... I do not know. What do you think ? What should we put into this reference vector ?
@PeterChmurciak
Here's a fast feedback after briefly reading your results and analysis.
- The ball speed estimate looks reasonable.
- The air speed estimate is weird, can't tell the reason at the moment. Might be the model, which is still imperfect in terms of air speed.
- Reference tracking does not function well - this may be only due to calculation of the LQ gain, i.e. tuning or model formulation. In particular, the state-space model should be appropriately augmented (add integrator state) for a offset-free reference tracking, which may be the issue here. I think there is likely a problem with both model and tuning.
- I am not the author of the LQ code, so I would have to look into that. But basically I agree with your logic, LQ followed by KF. In the meantime, try to have look at the lqg and lqi code built in MATLAB.
- For the reference states calculation, you are right with the first two. With the air speed, we would have to calculate its steady-state values corresponding to the values of the first two reference states. This can be done either experimentally (is it still an open problem or can we already do that for the sake of this?) or analytically (by calculating steady values of states form steady values of outputs/inputs using the state-space /still not a perfect/ model - this is my assumption and shall be checked first in the literature).
@gergelytakacs @mgulan I have tried to implement the integrator as you both suggested, and possibly was even sucessful. Also tweaked the model and both pairs of R,Q matrices.
Tweaked model and matrices are part of matlab example intended for calculating LQ gain for arduino example.
This is how it looks:
Current ball velocity estimate: (comparison)
Current ball velocity estimate: (detail)
Current air velocity estimate:
I have probably overdid it a bit with Q matrix values of ball and air velocity error covariances, but as a result I got more believable estimates of ball and air velocities.
Results from Arduino LQ example:
(Note: Time for stabilisation on each level is 1200 samples with 0.025 sampling period - 30 seconds)
As with PID example, there is more unwanted movement in the upper part of the tube. The results however are very good, much better than the ones in my previous post.
There is certainly room for some additional possible model and R,Q matrices tweaks, but these are the best results so far.
@PeterChmurciak Now it looks great; excellent job🙂 one thing that I don’t get is the control input profile shown in green. Is that only a perturbation that is then added to some reference input? Haven’t looked into the code yet.
@PeterChmurciak Good job!!! Excellent!:) We discussed everything in live chat, so I'm not going to reiterate.
The output (orange) and reference (blue) are in millimetres - in range 0mm to ~325mm, while the input is in percentages - in range 0% to 100%, that is the reason the green line is lower than the others. Only clarifying the difference.
@mgulan
As I understand it, due to current values of penalisation matrices, input is repressed, states representing ball position and velocity are repressed, integrator state is partly repressed while the air velocity - our least understood state, is the decisive state with the most weight. The controller tries to controll it to 0, and most probably that causes all those pertubations you mention around the steady value. The steady value is caused by integrator included in the LQ controller that almost constatly holds the value of the input aroud 45%.
Therefore most probably the integrator secures the steady value of input while air velocity being controlled to 0 causes those pertubations. I have tried different combinations of state weights, but the control was often unstable - again resembling tennis match in the tube. I believe it is precisely because of those pertubations that the ball can be stabilised so quickly and remain stabilised without diverging.
@gergelytakacs @mgulan I have to say that working with arduino through simulink is painful. It is almost like a guessing game on how to satisfy the compiler. The support for arduino is nowhere near perfect. Nevertheless I was somehow sucessful in creating LQ example in simulink. To be able to run it on Uno, as there were problems with memory even with PID, I had to create simplified blocks for this purpose. It is only so so, but it works on Uno.
Results look:
The part at the beginning is calibration, and at the end there were some false readings.
Results are pretty good.
@PeterChmurciak Regarding your previous post.. most of it makes sense, thanks for clarification. Nevertheless, it would be nice to back up what you suppose (regarding the perturbations etc.) in a "written" (mathematical) form, to make it understandable. I was merely curious why the input profile has a "constant" trend, which may be easy to explain. In any case, the controller is working correctly and the results are good.
Btw, off the top of my head, I have two math-related questions:
- Have you done the model augmentation properly? In the code, is the minus before C in matAhat = [matA, zeros(3, 1); -matC, 1] correct? Mind that I have not inspected it, I am just guessing.
- Does the fact that the used linearized state-space model is defined in delta variables (i.e. deviations from respective operating /linearization/ points, see the paper) somehow affect the way how you implement the control action?
is the minus before C in matAhat = [matA, zeros(3, 1); -matC, 1] correct?
Hmm, this is how I understand it:
The minus only represents that the integrator state will increment:
xInt = xInt + (ref - C*x)
If the minus was not there it would probably mean that the integrator increments:
xInt = xInt + (C*x - ref)
So as far as validity of computations go, it is possible that the sign does not matter as long as the integrator state is then incremented properly. Is that right or am I missing something ?
Does the fact that the used linearized state-space model is defined in delta variables somehow affect the way how you implement the control action?
Well... Probably not at all. Now that you mention it, it is very well possible that I am using the model incorrectly - as I am using it to calculate final output and not only its change, and the fact that it works anyway might be thanks to penalisation matrices...
@PeterChmurciak Alright, the former sound correct, the latter - I will have a look at the code.
He uses the absolute form for control, the input is just badly scaled. Look at our paper for IFAC, where I show the inputs in the range of 50-60% and even then I filter it through so some sort of level changes are visible. I think Peter's implementation is correct, he just needs to separate output/input, scale it, maybe filter it (for visualization only).
@gergelytakacs
That’s indeed what I have been referring to in other thread earlier today.
There’s no doubt the implementation is correct, it just needs to be presented as you suggested to make it clear;)
@mgulan I have not understood your questions properly before and because of my confusion, was not able to answer them. Mr. @gergelytakacs offered me his interpretation today so I think I know what bugged you on the results.
As @gergelytakacs suggested, I have filtered MPC results from other thread and cut part of it out:
First plot shows reference and original/filtered altitude, while second plot shows filtered input and third shows unfiltered input. The inputs did not look good on same plot due to different scale.
Although it is not obvious at first glance (or second or third...), under all the "noise" shown on third plot, is hidden signal shown on second plot, that actually realises the level changes.
I will try to reduce the noise with penalisation matrices, in ideal case the signal should look more like the filtered one, but I am a bit sceptical based on previous tweaking of penalisation matrices.
@PeterChmurciak never mind; what Gergely wrote earlier today together with the illustration above clarified it👍🏻 It was quite simple after all.
If you dont obtain better results in few tuning trials, I suggest to let it be.
@PeterChmurciak I have read through this discussion again. I find nothing of concern, your results look very good and I'm proud of your work. Now you should put all that to paper, since that is also an essential requirement to graduate. Please be sure you save all data from these experiments in well-kept MATLAB files - so you can plot them for your thesis.
@mgulan I am also thinking that as soon as MPC starts to make sense, we may start to think of a journal paper version of the IFAC contribution, that is expanded by Kalman, LQ, MPC etc. This is not @PeterChmurciak 's concern at the time, I'm just saying to calculate with that possibility in the future.
@PeterChmurciak I will review your code and solutions (unfortunately I don't have a bong shield right now) and will let you know if I find any (possibly minor) things to alter.
g.