Problem Statment
The controller can be only as good as the model of the system and the accuracy of the sensors data received by the controller. Ideally the sensor data should be clean and noise free without any discrepancy. But in practice there is some noise and error in sensor data.
This project investigates the later case. We explore possibility of noisy state value estimation, or measurement of every single state may not be available. We set out to design and implement an observer to come up with our own estimate of the system state, xˆ, which would then used by the baseline controller. We performed comparison of the ideal scenario with the base line controller and various observer designs.
We used MATLAB to design and quantify the observer controller pair. Final simulations of the quadrotor are done on PX4 simulator framework
Quadrotor model
The quadrotor model is non linear but we have linearized at the stable operating point. The focus of this project is not on non-linear quadrotor model but on linear and non-linear observer.
Base Line Model
We designed a simple LQR controller with linear quadrotor model and no observer. The controller is designed in MATLAB and then it is implemented in PX4 to quantify its performance in the simulated environment.
LQR parameters
Matrix Q has diagonal elements as [0.5, 5, 0.5, 5, 0.5, 5, 1, 1, 1, 1, 1, 1].
Matrix R is identity matrix.
State vector is given by: [u, pn, v, pe, w, h, p, φ, q, θ, r, ψ].
Q vector is chosen to penalise the error is displacement 10x than the penalty for velocity in that direction.
Parameters for angular displacement and velocity are set to 1.
The gravitational effect is nullified by feed forward control scheme by providing a fixed thrust to the quadcopter to overcome gravity.
The response time in the simulation is greater than the theoretical response time due to the upper limit on the propellor speed.
The error settles to around -0.15 in the simulation due to the external environmental factors in the simulator that are not taken into consideration while designing the model.
Defining State Space
Applying small-angle identities for the pitch and roll and linearizing f(x,u) around the equilibrium states and input, we have approximations for A and B.
And at any given time, the input to the system is determined by the state feedback control law - u = −Kx
where K was calculated using MATLAB’s lqr function and Q and R matrices designed using Bryson’s rule
Linear Observer Design
Experimentation in MATLAB
Calculated L matrix and experimented with all states annd no noise. The start position was (-15, -15, -15) in xyz respectively. Initial roll and pitch angles were 0. Yaw angle was -10 degree. The final setpoint given to the controller is origin with all angles 0.
As this ideal scenario, expected trajectory matches the actual trajectory very closely.
Experimentation in PX4
PX4 allows to simulate environment and sensor parameters to perform various tests on the controller .the LQR controller module uses the zero-order hold method to track and update our estimate for the states with the help of linear observer. Matrix L in calculated in MATLAB and fed into PX4 simulator.
Sensor Noise Injection
In order to quantitatively evaluate our observer’s performance in PX4, we altered the PX4 EKF state estimator’s measurements so that y would not match the vehicle’s state exactly, allowing us to analyze the response of our observer to inaccurate measurements. To the EKF signal, we added a vector of Gaussian noise with zero-mean and standard deviations appropriate for each state. Three different noise levels were tested as follows -
We feed in this noisy signal to the observer to estimate the actual state. We also fed the noise signal to baseline model without observer to compare the effects of having a linear observer in the system. The tracjectory starts at origin and ends at (5, 5, -5) with no change in roll, pitch and yaw.
Another comparison is done for various noise levels fed in to the linear observer
Finally to check the robustness of the observer, a complex pre programmed trajectory is fed in to the controller. Performance with noise injection is compared to the ideal trajectory.
Non-Linear Observer Update
Since our system dynamics are nonlinear, we also investigated the effect of using the nonlinear dynamics function in the estimator update equation instead of the linear state space model. In this approach, the observer update is defined by:
We assume small perturbations in the state vector to simplify the non linear update. This is then fed to the LQR controller and the performance of linear and non-linear observer is compared for various noise levels. In all experiments, it was observed that non-linear observer is more stable with faster response time and less steady state error.
Conclusions
A stable observer-controller can be implemented in PX4 simulator with different sensor noise to quantify robustness of the system.
Linear observer controller works in an ideal system (Simulink) implementing a state-estimating observer in the more realistic testing environment of PX4 proved to be far more challenging.
One of the reason for poor performance than theoretical simulations was coordinating the updating of the linear observer values to feed into the controller, and accounting for the delays inherent in using a naive zero-order hold discretization.
We observed robust non-linear observer that estimates states better has better performance than naive controller.
To generate estimates for states that are not directly reported in the output, we are interested in implementing a sliding mode observer which is better-suited to account for the non-linear dynamics of the system in order to construct estimates of these states.
References
Beard, Randall. Quadrotor Dynamics and Control. Brigham Young University. (2008)
A. E. Bryson, Control of Spacecraft and Aircraft. Princeton, NJ: Princeton University Press, (2015)
Åström, Karl J. and Richard M. Murray. Feedback Systems: An Introduction for Scientists and Engineers, Second Edition. Princeton, NJ: Princeton University Press, (2020). Retrieved at http://www.cds.caltech.edu/ murray/amwiki/index.php/Second_Edition.
Moura, Scott. “Chapter 2: State Estimation”, CE 295: Energy Systems and Control. https://ecal.berkeley.edu/files/ce295/CH02-StateEstimation.pdf
Manon Kok, Jeroen D. Hol and Thomas B. Schön (2017), “Using Inertial Sensors for Position and Orientation Estimation”, Foundations and Trends in Signal Processing: Vol. 11: No. 1-2, pp 1-153. http://dx.doi.org/10.1561/2000000094
S. V. Drakunov, “Sliding-mode observers based on equivalent control method,” [1992] Proceedings of the 31st IEEE Conference on Decision and Control, Tucson, AZ, USA, 1992, pp. 2368-2369 vol.2, doi: 10.1109/CDC.1992.371368.