EstimatorRegulator
OutputResponse
StateResponse
NonlinearStateSpaceModel
Solve

Control Systems
Engineering
System Modeling

Track the Altitude of a Drone

Design an estimator-regulator tracking controller for a quadcopter drone to set its altitude to a desired value

Open in Cloud
Download Notebook

Here are the drone's equations for the rotational accelerations around it's principal axes correspond to the angles

and

In[1]:=

′′

[t]

ℐ

ℓ

[t]-(Ω

)

′

[t]+

ℐ



′

[t]

′

[t];

′′

[t]

ℐ

)ϕ'[t]ψ'[t]

ℐ

(ℓ

[t]-(Ω

)

′

[t]);

′′

[t]

ℐ

ℓ

[t]+

ℐ



′

[t]

′

[t];

Set up the equation for the translational acceleration along the

-axis:

In[2]:=

′′



[t]-g+

ρs

'[t]

(Cos[θ[t]]Cos[ϕ[t]])

[t]

;

In[3]:=

eqns=

′′

[t]

ℐ

l

[t]-(Ω

)

′

[t]+

ℐ



′

[t]

′

[t],

′′

[t]

ℐ

[t]-(Ω

)

′

[t])+

((-

ℐ

)

′

[t])

′

[t]

ℐ

′′

[t]

ℐ

l

[t]+

ℐ



′

[t]

′

[t],

′′



[t]-g+

ρs

'[t]

(Cos[θ[t]]Cos[ϕ[t]])

[t]

;

Compute the equilibrium points of the system by first setting the initial conditions to

t=0

In[4]:=

eqns/.{Derivative[_][_][t]0,_[t]0}

Out[4]=

{True,True,True,0-g}

Solve the differential equation for the vertical acceleration, along the

-axis, with the given initial conditions:

In[5]:=

eqns〚-1〛/.{Derivative[_][_][t]0,(θ|ϕ)[t]0};

1eq

[t]/.Solve[%,

[t]]〚1〛

Out[5]=

Verify the solution:

In[6]:=

eqns/.

[t]

1eq

/.{Derivative[_][_][t]0,_[t]0}

Out[6]=

{True,True,True,True}

Compute the numerical values for the model parameters:

In[7]:=

parameters={

0.43`,

ℐ

0.0196`,

ℐ

0.0196`,

ℐ

0.0264`,l0.25`,

0.01`,Ω475,g9.8`,s0.0314,ρ1.226,

0.47};

A nonlinear state space model of the quadcopter drone:

In[8]:=

drone=NonlinearStateSpaceModeleqns,

states

ins

outs

,t/.parameters

Out[8]=

ϕ[t]	x. 1 [t]
x. 1 [t]	0.0196(-4.75 x. 2 [t]-0.0068 x. 2 [t] x. 3 [t]+0.25 u 2 [t])
θ[t]	x. 2 [t]
x. 2 [t]	0.346939 x. 1 [t] x. 3 [t]+0.0196(-4.75 x. 2 [t]+0.25 u 3 [t])
ψ[t]	x. 3 [t]
x. 3 [t]	0.0264(0.+0.25 u 4 [t])
[t]	x. 4 [t]
x. 4 [t]	-9.8+0.00904665 2 x. 4 [t] +2.32558Cos[θ[t]]Cos[ϕ[t]] u 1 [t]
	[t]
	ϕ[t]
	θ[t]
	ψ[t]

We can see that without any input, the drone is in free fall:

In[9]:=

OutputResponse[{drone,{0,0,0,0,0,0,100,0}},{0,0,0,0},{t,0,4}];Plot[%〚1〛,{t,0,4},PlotRangeAll]

Out[9]=

Set the output



, the altitude, as the tracked output:

In[10]:=

sspec="InputModel"NonlinearStateSpaceModel[drone,Automatic,Automatic,Automatic,None],"TrackedOutputs"1;

Compute a set of estimator gains for the set of poles

In[11]:=

p={-15,-2+,-2-,-1.5+,-1.5-,-1+0.8,-1-0.8,-2};

In[12]:=

MatrixForm[ℓ=EstimatorGains[sspec,p]]

Out[12]//MatrixForm=

0.940851	10.0255	-6.47502	-0.524235
1.11086	11.1338	-6.74923	-0.771602
-0.024993	-8.30256	9.75409	0.759275
-0.67633	-13.6458	13.8157	1.29917
-0.106558	-0.490433	0.533628	3.04479
-0.0874215	-2.43138	2.28396	3.44716
3.0825	-12.6311	11.2454	0.928301
1.86818	-28.8065	26.456	1.77759

And an LQ regulator using the set of weights

wts

In[13]:=

wts=

1	0	0	0	0	0	0	0	0
0	10	0	0	0	0	0	0	0
0	0	1	0	0	0	0	0	0
0	0	0	10	0	0	0	0	0
0	0	0	0	1	0	0	0	0
0	0	0	0	0	10	0	0	0
0	0	0	0	0	0	1	0	0
0	0	0	0	0	0	0	10	0
0	0	0	0	0	0	0	0	10

IdentityMatrix[4];

In[14]:=

lqr=LQRegulatorGains[sspec,wts,"Data"]//Chop

Out[14]=

SystemsModelControllerData

Design: linear quadratic regulator

Feedback inputs count: 4



Assemble the estimator regulator:

In[15]:=

ℯ=EstimatorRegulator[sspec,{ℓ,lqr},"Data"]

Out[15]=

SystemsModelControllerData

Design: estimator and regulator

Measured outputs count: 4



Obtain the closed-loop system:

In[16]:=

csys=ℯ["ClosedLoopSystem"]//Simplify//Chop

Out[16]=

ϕ	◼
x. 1	◼
θ	◼
x. 2	◼
ψ	◼
x. 3	◼
	◼
x. 4	◼
{ x. 7 ,0}	◼
{ x. 9 ,0}	◼
{ x. 6 ,0}	◼
{ x. 10 ,0}	◼
{ x. 8 ,0}	◼
{ x. 11 ,0}	◼
{ x. 5 ,0}	◼
{ x. 12 ,0}	◼
x. 13	◼
	◼
	◼
	◼
	◼

The drone tracks the reference altitude

ref

In[17]:=

ref={10};sr=StateResponse[csys,{ref},{t,0,100}];Plot{%〚7〛,ref},{t,0,10},

plotOpts



Out[17]=

	output response
	reference

Obtain the controller model:

In[18]:=

cm=ℯ["ControllerModel"]//N//Simplify//Chop

Out[18]=

{ϕ,0}	◼
{ x. 1 ,0}	◼
{θ,0}	◼
{ x. 2 ,0}	◼
{ψ,0}	◼
{ x. 3 ,0}	◼
{,0}	◼
{ x. 4 ,0}	◼
x. 5	◼
	◼
	◼
	◼
	◼

Specify the controller inputs:

Source Metadata

Citation:
- Quadcopter Flight Mechanics Model and Control Algorithms, Gopalakrishnan, 2016.[link]

Related Symbols

EstimatorRegulator
OutputResponse
StateResponse
NonlinearStateSpaceModel
Solve

Publisher Information

Contributed by: Wolfram Controls Team

Wolfram Language Example Repository

Track the Altitude of a Drone

Source Metadata

See Also

Publisher Information

1	0	0	0	0	0	0	0	0
0	10	0	0	0	0	0	0	0
0	0	1	0	0	0	0	0	0
0	0	0	10	0	0	0	0	0
0	0	0	0	1	0	0	0	0
0	0	0	0	0	10	0	0	0
0	0	0	0	0	0	1	0	0
0	0	0	0	0	0	0	10	0
0	0	0	0	0	0	0	0	10

1	0	0	0	0	0	0	0	0
0	10	0	0	0	0	0	0	0
0	0	1	0	0	0	0	0	0
0	0	0	10	0	0	0	0	0
0	0	0	0	1	0	0	0	0
0	0	0	0	0	10	0	0	0
0	0	0	0	0	0	1	0	0
0	0	0	0	0	0	0	10	0
0	0	0	0	0	0	0	0	10

1	0	0	0	0	0	0	0	0
0	10	0	0	0	0	0	0	0
0	0	1	0	0	0	0	0	0
0	0	0	10	0	0	0	0	0
0	0	0	0	1	0	0	0	0
0	0	0	0	0	10	0	0	0
0	0	0	0	0	0	1	0	0
0	0	0	0	0	0	0	10	0
0	0	0	0	0	0	0	0	10