## Sunday, October 30, 2016

### Linear Gaussian Quadratic Control System

For the last year when I've had time, I have been developing an entirely new control scheme (at least as far as I know for the open source community) for multi rotors that replaces PID by expanding upon Tau Labs System Identification (see the mathematical description here).

The way linear quadratic control (LQG) works is by simultaneously estimating additional properties in the system we cannot directly measure (in this case the instantaneous torque and true rate of rotation) and the using those to control the system. I had previously experimented with something a bit ad hoc like this in the past.

## System Identification

A while ago Korken proposed a nice EKF that would extract a few critical properties of the system (primarily the 'gain' of each axis and the time constant of the motors). I wrote up an implementation of this and we got it running and measuring reasonable values. Based on this, I came up with some analytic approaches to optimize the PID control parameters based on these estimated system properties.

More specifically we use a model with several parameters that are specific to each frame $\beta_1$, $\beta_2$, $\tau$. The latency of the ESC/Motor/Prop is captured by $\tau$ and is one of the most performance limiting factors in a frame. For roll and pitch only $\beta_1$ is used and corresponds to the strength of that control output. For yaw there is a component that has no latency -- $\beta_2$ -- and that corresponds to directly generating a reaction force torque by accelerating the props.

$\dot \theta = \omega$
$\dot \omega = \beta_1 \cdot \nu + \beta_2 \cdot \left(u_c - \nu \right)$
$\dot \nu = \frac{u_c - \nu}{\tau}$

The EKF above will estimate these parameters for a frame by having it shake while flying.

## Online estimation

To perform linear quadratic control (LQR) we need to be able to estimate $\nu$ which is the instantaneous (normalized) torque on the frame. In addition, we would like to estimate $\omega$ -- the true rotation rate -- although we have the noisy measured values of it from gyro.

We can take the above system dynamical equations and convert it to a continuous time state space representation. Because the output may be slightly biased we also need to include this into the state estimation $b$. In addition, this estimator will not estimate the angle as we will take that from either the complementary filter or the INS.

$\mathbf x = \left( \begin{matrix} \omega \\ \nu \\ b \end{matrix} \right)$

We can the write down the equations above (including bias) as

$\dot {\mathbf x} = \mathbf A_c \mathbf x + \mathbf b u + \mathbf w$

$\mathbf A_c = \left( \begin{matrix} 0 & \beta_1-\beta_2 & -\beta_2 \\ 0 & -1/\tau & -1/\tau \\ 0 & 0 & 0 \end{matrix} \right)$

$\mathbf b_c = \left( \begin{matrix} \beta_2 \\ 1/\tau \\ 0 \end{matrix} \right)$

Of course since we are going to be implementing this in discrete time, we need to convert from the continuous time representation.

$\mathbf x_{t+1} = \mathbf A_d \mathbf x_t + \mathbf b_d u_t + \mathbf{w}_d$

$\mathbf A_d = e^{\mathbf A_c T_s} = \left( \begin{matrix} 1 & A_{d12} & A_{d13} \\ 0 & e^{-T_s/\tau} & e^{-T_s/\tau} - 1 \\ 0 & 0 & 1 \end{matrix} \right)$
$\qquad A_{d12} = \beta_1 \tau - \beta_2 \tau - \beta_1 \tau \exp(-T_s/\tau) + \beta_2 \tau\exp(-T_s/\tau) = (\beta_1 - \beta_2) \left(\tau - \tau e^{-T_s/\tau}\right)$
$\qquad A_{d13} = - T_s \beta_1 + \beta_1 \tau - \beta_2 \tau - \beta_1 \tau e^{-T_s/\tau} + \beta_2 \tau e^{-T_s/\tau} = - T_s \beta_1 + A_{d12}$

$\mathbf b_d = \int_0^{T_s} e^{\mathbf A_c s} ds \, \mathbf b_c = \left( \begin{matrix} -A_{d13} \\ 1 - e^{-T_s/\tau} \\ 0 \end{matrix} \right)$

$\mathbf c = \left( \begin{matrix} 1 \\ 0 \\ 0 \end{matrix} \right)$

Because this system is a linear time-invariant Kalman filter, the covariance matrix will converge to a constant regardless of the observations and can be used to pre-compute a kalman gain. This requires solving the discrete algebraic Ricatti equation (DARE) to estimate the steady state posterior noise distribution (details omitted). With this we get an equation for the estimator.

$\hat {\mathbf x_{t+1}} = \mathbf A_d \hat{\mathbf x}_t + \mathbf b u_t + \mathbf l \left( y - \mathbf c^\mathrm{T} \hat {\mathbf x}_t \right)$

$\mathbf l = \mathbf P \mathbf c \left[ \mathbf c^\mathrm{T} \mathbf P \mathbf c+ r\right]^{-1}$

Which means once we have solved the DARE and for $\mathbf l$ the kalman filter only requires several multiplications and addition operations to update. When controlling attitude we use the angle estimate from the original complementary filter or INS algorithm.

 Online estimation running during an autotune session. The green line in the top plot shows how much the gyros are denoised by the Kalman filter. The bottom plot shows the online instantaneous torque estimation, which is used to replace the original derivative term in PID.

## Control algorithm

Armed with the state estimate we can use a linear quadratic regulator (instead of traditional PID) to compute the desired output. Here we talk about controlling the attitude. This controller has the form

$u = -\mathbf k^\mathrm{T} \hat {\mathbf{x}} + b$

Where $\mathbf k$ is optimized to minimize a cost function $J$ and $x=\left( \begin{matrix} \theta \\ \omega \\ \nu \end{matrix} \right)$.

$J=\sum_{t=0}^{t=\infty}{\mathbf x_t' \mathbf Q \mathbf x_t + u_t r u_t}$ -- this uses $u$ without the bias added in.

$\mathbf x = \left( \begin{matrix} \theta \\ \omega \\ \nu \end{matrix} \right)$

Again converting the continuous dynamics to discrete time gives a system

$\mathbf A_d = \left( \begin{matrix} 1 & T_s & A_{d13} \\ 0 & 1 & A_{d23} \\ 0 & 0 & e^{-T_s/\tau} \end{matrix} \right) \\ \qquad A_{d13} = \tau (\beta_1 - \beta_2) (\tau e^{-T_s/\tau} - \tau + T_s) \\ \qquad A_{d23} = \tau (\beta_1 - \beta_2) (1 - e^{-T_s/\tau})$

$\mathbf b_d = \left( \begin{matrix} T_s^2 \beta_1 / 2 - A_{d13} \\ T_s \beta_1 - A_{d23} \\ 1-e^{-T_s/\tau} \end{matrix} \right)$

We can then plug these dynamics into DARE, as well as the parameters $\mathbf Q$ and $r$ from above to which determine performance tradeoffs. From that we can solve for $\mathbf k$.

$\mathbf k^\mathrm{T} = (r + \mathbf b^\mathrm{T} \mathbf P \mathbf b)^{-1} (\mathbf b^\mathrm{T} \mathbf P \mathbf A)$

### Costs

The parameters in the $\mathbf Q$ matrix ultimately determine the system performance -- how aggressively it attempts to drive the state to the setpoint (which will be discussed below). In this system we use a diagonal $Q = \mathrm{diag}(q_1 (1-s), q_2, q_3)$ which allows us to write the cost function a little bit more naturally.

$J=\int_{t=0}^{t=\infty}{q_1 \theta^2 + q_2 \omega^2 + q_{3} \nu ^2 + u_c^2 r \, \mathrm{d} t}$

This expression makes it clear that by having $q_1$ greater the system will more aggressively try and correct angular errors. By having $q_2$ greater it will also try and keep $\omega$ closer to zero when doing that, which has the effect of smoothing out the response. Similarly setting $q_3$ greater will attempt to drive the error to zero with the minimal amount of torque possible. Both of these will smooth the response out and lessen overshoot.

Because we are optimizing the feedback gains ($\mathbf k$) to minimize this function, we can divide out r -- or without loss of generality set it to 1 -- without changing $\mathbf k$.

### Setpoints

To use LQR to control either rate or attitude (as opposed to drive both to zero) we need to redefine error from setpoint as

$\mathbf x_{r} = \mathbf x-\mathbf r$

which leads to us having a new control rule

$u=-\mathbf k^\mathrm{T} \mathbf x_r + b$

In order to have an attitude controller, we want

$\mathbf r_\theta = \left( \begin{matrix} \theta_r \quad 0 \quad 0 \end{matrix} \right)^\mathrm{T}$

In order to have a rate controller, we similarly change the setpoint reference to control the angular rate state and set the angle to zero

$\mathbf r_\omega = \left( \begin{matrix} 0 \quad \omega_r \quad 0 \end{matrix} \right)^\mathrm{T}$

and then set $q_{0,0}=0$ in order for the LQR controller to not penalize the angle not being at zero. This could be equivalently done with a two state system but would just complicate the code (and prevent doing things like smoothly transitioning from rate to attitude control, like in horizon mode).

### Frame invariant behavior and costs

Without getting too much into the derivation, by slightly tweaking the cost parameters so that $q_3 = \beta_1 q_4$ we can ultimately generate responses that are much more similar across frames regardless of their parameters. It will never be possible to get exactly identical performance if we are pushing frames to the limits of what they can do as some performance simply is not achievable on a more sluggish frame.

I simulated the closed loop response when changing parameters and using this representation of the cost and show that fairly consistent closed loop responses in rate mode would be achieved across a wide variety of frame types.

 The response and control output for a step response with $\beta=8$ and $\tau=-3.5$ is shown here.

 This shows a set of simulated step responses in rate mode when sweeping the $\beta_1$ parameter. Although there is a slight change in the shape, it is fairly minimal.

 Similarly as we sweep $\tau$ there is also only a small change in the simulated closed loop performance.

Having this type of invariance has the benefit that once some more serious pilots and tweakers find a set of costs that work well for them it should apply more -- both when using them on different frames and sharing them amongst other pilots.

### Real world results

As usual when testing these kind of control algorithm, I threw Freedom on a frame, which is a flight controller that has an Overo Gumstix on it that lets me log all the sensor data in the flight. I can then analyze the filter offline (as well as reprocess the data to refine things like the filter).

Here it is tied up to isolate one axis so I could test extremely high rotational speeds that I couldn't otherwise indoors.

A video posted by peabody124 (@peabody124) on

And from that I can download the logs and look at the performance when doing fast movements in attitude mode as well as in rate control.

 Here you can see me switching between rate and attitude control. In both cases the responses are extremely crisp.

Here is some data from a free flying frame.

 The green points in the top trace show the desired angle and the blue curve is the actual angle. The system responds very promptly to the input stimuli. The bottom trace again shows the estimator that again smooths the gyro down.

This graph shows that the system responds very promptly. Because of the smoothed gyro and torque estimates, LQG can tolerate much greater effective gains than can work with PID control and no smoothing.

## So why LQG?

After going through all this, it is worth taking a moment to ask why do we want to implement this. There are two main benefits corresponding to the two halfs of LQG.

The first part is the benefit of the estimation component. By running the Kalman filter we fuse together the inputs that we are sending to the motor and the noisy gyro estimates to get the best possible estimate of what is really going on. Superficially, this has the benefit of significantly denoising the gyro signal -- after all that very high frequency noise is physically implausible but applying a low pass filter to it introduces latencies that people are always trying to chase out. In addition, it allows us to directly estimate the instantanous (normalized) torque being generated by the motors. This comes from the latency of their responses and allows us to properly damp the control algorithm rather than the ad hoc derivative term typical in PID. The derivative is critical for good performance and those who have been in this game long enough to remember when the bandlimited derivative from APM was ported over to OpenPilot recall how much of a performance boom it was. However, that band limiting is required because of the amount of noise when directly taking the difference of gyro measurements. The state estimation reduces the amount of noise there allowing much greater derivative terms. When looking at the parameters as the corresponding PID terms we can use much much higher PID parameters when using the state estimator. In practice I'm using a factor of 10 higher values than without this estimator.

The second half is the actual calculation of the gains. As I referenced in the beginning after we got the system identification going I came up with some analytic equations to optimize the PID coefficients from this (derivation is here). This has worked well and feedback on Tau Labs autotuning  has always been quite positive. However, LQR is a much more principled and mainstream approach to the same problem. It is more computationally intensive, but by only having to calculate the DARE equation once given the system identification parameters, we can do this before take off and avoid this cost in real time. Thus the costs of running this LQG is essentially the same as PID.

## Conclusion

LQG is a more modern control scheme than PID. It flies much better. This implementation simply requires a regular autotune flight, but instead of using the math I previously developed that is now used by TauLabs/LibrePilot/Dronin it directly computes LQR coefficients. Here I show it flying on 3 frames of much different sizes without issue:

1. Control system are important and are present almost everywhere in our daily lives. Control system provides an output or response for a given input or stimulus.
Transformer Manufacturer in India

2. Excellent. Any chance of this making it into the flight code in the near future?

1. OK I see it in gitHub.

3. Glowtape from IRC here...

I've implemented the math as a plain rate controller (no angle component) over this weekend, and by chance I got a dry day today.

I had to spend quite a while in MATLAB to figure out Q and R for the RTKF. Same for Q for the LQR, for trying to get sane step responses. Especially latter seems confusing, due to the difference in magnitudes. My current Q for rate is 0.00001 and the Q for torque is 0.5*non-exp Beta (e.g. 0.5*10.77 for one axis).

Despite working OK in the living room with simple hover and actuations, I figured this can't be right.

But then I had a bout of sunshine today and went outside. Like holy f%@#, this works so damn well. Especially extreme manoeuvers like flips, it stops on a dime.

Awesome work!

It is actually the quality of the academic essay writing service which has made us popular in the world. Our services are made in such a way that you don’t have to go through a long process in order to get our cheap writing service. Our services are made in a very simplistic way and everything about us is explained on our website. You can check our cheap academic essay writing service and see if everything we told you was the truth or not. You get the help of the best academic essay writers if you choose us. We have gained the satisfaction of thousands of students. You can see the rating on our website. To provide you the best quality, we have hired the experts who are highly qualified. They have the experience and the skills which are required to ensure good quality essay. You can simply ask our experts online anything concerning the essay and they will debrief you properly. Along with quality, there are many other aspects and packages which we offer you for your satisfaction. https://liveassignmenthelp.com

5. Sample Assignment is a renowned for assignment help in Australia and has assisted thousands of international students with their academics. Our dedicated team of experts has been providing full-fledged assignments to students pursuing their courses at various colleges and universities, and found to be avidly googling "assignment help Sydney" across the continent. While Australia is among the most preferred destinations for individuals from around the world, Assignment Experts has also won the trust of a vast pool of students here. The Australian Assignment help, such as Sample Assignment, can be easily contacted via WhatsApp and Messenger too. With our 24-hour online academic assistance, any student can reach out to us whenever he or she is in the need of help of a subject expert.

6. Hello. I appreciate your concern. However, there are numerous Australian assignment help service provider in Australia that are offering their professional assistance to students in their academic assignments.
Though, I would like to suggest you about an assignment writing service provider that goes by the name My Assignment Help Oz. MAHOZ consist of a specialised team of experts who knows everything about marking rubrics and formats. Also, the team of experts are PhD holders from reputed universities in Australia and knows every microscopic detail about their respective course study.
Thus, they can provide students with best Australian Assignment help services.
In addition, the team of writers claims to be best in preparing: essays, reports, articles, dissertations, theses, case study etc. They also acquire knowledge in various subjects like management, nursing, accounting, psychology, engineering etc.
Thus, all who availed their services are pretty much satisfied and mostly all the students are very happy with the kind of quality solutions that they prepare, well-before stipulated deadlines, making them prime Australian assignment help provider.

7. Concursos públicos no Brasil. Concursos Abertos, previstos 2019 e concurso em Andamento. Confira Edital, Simulados e Provas de concursos anteriores. Entre em contato com o Concursos para mais informações. concursos abertos 2018, Lista de Concursos Públicos no Brasil com inscrições abertas em todo o país. Confira a lista de todos os pci concursos abertos no país em e 2018.

8. At Justquestionanswer.com experts are ready to provide you do assignment online. The best team of masters and Ph.D. degree professionals that understand student’s demands and complete the task properly. Our experts of native writers from the USA.

9. Cursorily, this has the advantage of fundamental denoising the gyro signal - after all, that exceptionally high-recurrence commotion is Shared Office Space Dubai genuinely doubtful yet applying a low pass channel to it presents latencies that individuals are continually attempting to pursue out.

1. Magic tyres is the best tyre shop in Dubai they provide professional services for your car tyres besides the professional services their Tyres Prices is very low which is affordable for everyone

10. In control hypothesis, the direct quadratic–Gaussian (LQG) control issue is quite possibly the most central ideal control issues. Free Breakdown Service Near Me The issue is to decide a yield input law that is ideal in the feeling of limiting the normal estimation of a quadratic expense rule.

11. Chase Verify Card – Chase bank Credit Card clients can confirm and activate online at chase.com. In the event that you have just applied for a Credit Card of Chase Bank, you ought to activate it by confirming it online at Bank's authentic site is www.chase.com. On the off chance that you "How would I enact my interest Credit Card?" Are searching for. So read the cycle beneath to confirm the card, at that point activate it.

12. Hastily, this has the Erp Software UAE advantage of fundamentally denoising the gyro signal - all things considered, that high-recurrence commotion is truly impossible yet applying a low pass channel to it presents latencies that individuals are continually attempting to pursue out.

13. This reality sheet clarifies the guidelines for enormous amount generators (LQGs) of dangerous waste, which should consent to the full arrangement of government perilous waste guidelines. HR Software Companies In Dubai You are viewed as a LQG in the event that you produce per schedule month: More than 2,200 lbs (1,000 kg) of risky waste.

14. In control hypothesis, the straight quadratic–Gaussian (LQG) control issue is perhaps the most crucial ideal control issues. IOS Development In Pakistan The issue is to decide a yield input law that is ideal in the feeling of limiting the normal estimation of a quadratic expense standard.

15. This subject matter is pretty incredible. I am surprised by the content material you shared on this internet site. Further to Diesel Supplier In UAE, I've regularly made use of their live one-on-one consultation to get in contact with their professionals even after they submitted the whole work. This helped me a lot to make clear the standards even better.

1. Sina Tyres are the best tyres in UAE they provide tyre services as well as car tinting services They have very low Car Tinting Dubai Prices as compared to other car tinters.

16. Remembering some more terms for the summed up least fluctuation measure, treated in Exercise 11.5, and in this way punishing future yield deviations and control activities is regularly Pakistan Cargo Dubai called summed up prescient control. See Bitmead, R.R., Gevers, M., Wertz, V., 1990. Versatile Optimal Control. The Thinking Man's GPC. Prentice Hall International, Hemel Hempstead, UK.

17. The way straight quadratic control (LQG) works is by at the same time assessing extra properties in the framework we can't straightforwardly Resident Evil 2 Jacket quantify (for this situation the immediate force and genuine pace of revolution) and the utilizing those to control the framework.

18. The subsequent half is the real estimation of the additions. As I referred to in the first place after we got the framework distinguishing proof moving I concocted some insightful conditions to streamline the PID coefficients from this (induction is here). This has functioned admirably and criticism on Tau Immigration Consultants Dubai Labs autotuning has consistently been very sure. Nonetheless, LQR is a considerably more principled and standard way to deal with a similar issue.

19. This is my first time i visit here. I found so many entertaining stuff in your blog, especially its discussion. From the tons of comments on your articles, I guess I am not the only one having all the leisure here! Keep up the good work. by embroidery digitizer design

20. Remembering a few more terms for the summed up least fluctuation measure, handled in exercise I loved this web page so much that I have bookmarked it for destiny reference. Thank you for this precious piece of facts. Unluckily I could not locate any extra data in this web page.

Wheel Alignment In Dubai
Tyres Price

21. What the blog is missing out on is a superb conclusion, as it would have given it the perfect finishing touch. Now you can make most of the academic writing services at MyAssignmentHelpAU offering Programming Assignment Help. The services promise to add all elements your paper needs.

22. To play out the activation process you simply need a Primevideo.com/mytv account, where you can make a record on the Prime video in the event that you have not bought in to watch the Prime video. Keep in mind, you may likewise profit the great video of the I month preliminary membership. How about we discover the essential advances which will assist you with making the great video account.

23. A lot of accounting or finance students look for MYOB Assignment Help Looking into the high demand for MYOB services, Myperdiscohelp.com has started to deliver help regarding Perdisco and MYOB. So, if you are facing any difficulty in solving your accounting or finance calculations, come to MyPerdiscohelp.com and we would offer our premium quality help.

24. Its really so good and fresh to me and Your many others articles gives many helpful topic information. I have read several excellent stuff here. Certainly value bookmarking for revisiting. I wonder how so much effort you put to create such a fantastic informative website. Thank you and all the best. essay typer - homework help - Assignment help

26. ISO lead auditor training course is an essential training course for delegates who wants to become a certified ISO lead auditor. IRCA ISO Lead Auditor course in Delhi | Class room, In-house, online | 5-days | Certificate in 10 days | 95% rating | Excellent Feedback | 9962590571

27. Microsoft Office accounts can be access simply by the customer. If a user does not have an Office account, they can create one by visiting this page www.office.com/myaccount and customers who have an Microsoft account can click directly on the sign-in button to gain instant access.

Get trend micro from the official site via Trendmicro.com/bestbuypc and enter the trend micro activation key to complete the process. Creating a Trend micro account is also required for operator that helps in buying subscription, Activate Trend micro for Windows, Mac, Android and iOS.

Webroot.com/safe allows you to directly download, install and activate Webroot safe on your computer after you purchase a substantial membership online or in a retail shop.

28. Mcafee.com/activate is one of the most popular world class antivirus software which is widely known for offering high class security services to their valuable clientele. Make your buy online or buy it from any of the nearest offline store. Once you buying the McAfee product, you can easily download, install and activate it on your Device.

Need to Have Advanced Security software from www.avg.com/activation Software with the following Link avg.com/activate that help to protect all device from virus, malware and other online threats.

Webroot software is one of the most used antiviruses in the world. Webroot.com/safe antivirus keeps your System performance optimized and keeps your computer secured while surfing internet.

29. Hello there, I'm so happy I ran across this information. I'm seeking for a high-quality blog with engaging content. There are also some interesting sites nearby that you can visit whenever you like. login cash app | mcafee total protection | uninstalling mcafee windows 10

30. My Nane is Robert Smith, I am working in Qatar Airways office for custmenr assistance. If you are searching for Qatar Airways New York Office address
or Qatar Airways New York Office Phone Number then visit here Qatar Airways New York Office or simply call at +1-855-577-0157

31. Students often struggle to write persuasive essays. They keep thinking, “How can I write and university assignment help to pursue their readers?” And these students usually keep thinking about which argument to present for their essay’s favour?
Are you one of those students? Well, what you are told that it is not the arguments but the topic of the essays that matter? Would you like to know what those topics are? Then read on to know all about it. By the time you have finished reading this, you will have the perfect topic to work on your essay.
Humans are inherently good
All humans are fundamentally good. And their devilish natures are just a coping mechanism. This is one of the most complicated yet exciting topics for a persuasive essay. While French philosopher Jean-Jacques Rosseau believed that humans are inherently good, Thomas Hobbes had a different perspective.
Schools should have less technology
This is another excellent topic and should be more discussed upon. Recent research by the scientists at Michigan State University states that technology and the devices it gave birth are doing more harm than benefit.
While video sharing platforms like YouTube have multiple academic channels that help students with extra resources. Students seldom use accounting assignment help for their educational usage. They are more on these platforms for personal entertainment. Then again, technology has helped many students for arcgis assignment help. They can now share classrooms with many other students who do not have any outwardly learning lacking. Therefore, you can establish multiple arguments on this topic and make it more interesting for your readers.
Courtrooms should allow filming
In the United States of America and many other affluent parts of the world, filming in courtrooms is not allowed. While in the United States, this law differs from one state to other. The broadcasters initially filmed the famous OJ Simpson Murder trial in the USA. However, as the popularity of the case grew, the judges decided to ban cameras from the courtrooms.
Books should never be banned
Books are boons, and thus they should never be banned. Banning books can cause more harm than good to readers and their curiosity. Unfortunately, many books and literature have been banned in many nations. Books like The Communist Manifesto have been banned in many major countries globally, including the USA and South Korea.
These topics described here are some of the most controversial and exciting topics. So, if you write an entire essay on them and can pursue your professors, rest assured, you will become a great writer. Moreover, you would not need to worry, can I write and dissertation writing help properly? So, don't waste your time; go through these suggestions and write a persuasive essay.