In this section a controller for a linear system is designed using LQG techniques; see, for example, [1, 3]. The resulting filter is transformed into discrete form and C++ code is generated corresponding to the filter equations. An extremely simple simulation is used to compare the generated code with the uncompiled Mathematica functions.

We will consider a linearization of a nonlinear aircraft dynamics for some specific flight conditions. We study only horizontal motions.

Introduce the following notations:

• course angle

• velocity across the aircraft in the y-direction

• roll angle

• aileron deflection

• rudder deflection

and choose the state vector , where and .

Notation for a Linear Time Invariant (LTI) system is given in state-space form:

The following numerical instantiations of the system matrices comes from an example in [1].

Suppose that we have the following wind disturbance acting on the system.

How will the system respond to these disturbances? We plot a simulation of the system response for 10 seconds.

Figure 7. The response in roll angle (blue) and course angle (red) to the wind disturbances plotted in Figure 6.

We compute a state-feedback, that is, an matrix, by minimizing a quadratic criterion with weight matrices and . The quadratic criterion is given by

We play around with the matrices and until we are satisfied with the result.

The closed-loop eigenvalues are given.

A necessary and sufficient condition for asymptotic stability of the closed-loop system is that these eigenvalues belong to the left half-plane.

We simulate the closed-loop output response.

Figure 8. The response in roll angle (blue) and course angle (red) to the wind disturbances plotted in Figure 6 for the controlled system.

We compare the disturbance attenuation with (dashed) and without the controller.

Figure 9. A comparison between the response in roll angle (blue) and course angle (red) to the wind disturbances plotted in Figure 6 for the controlled (dashed) and uncontrolled system, respectively.

Now suppose that the states cannot be measured. Here we will design a linear dynamic controller known as a Kalman filter, which is used to estimate the states from measured signals.

Assume that the noise has the following covariance matrices.

We check the eigenvalues of the dynamic controller to verify that it is stable.

The Kalman filter equations are

The computed state-feedback control law is of the form

if we add a reference signal r.

Using the separation principle [1, 3], we can use the estimated states in equation (9) instead of the true states and still get an optimal design. In that case, equations (9) and (10) give the following LQG controller.

Here we choose such that the static gain from r to z becomes an identity matrix.

We represent the controller given by equation (11) in state-space form.

The continuous Kalman filter above is transformed to discrete time for implementation purposes.

We define a simple test signal, which corresponds to a unit step in the reference signal for the roll angle while keeping the reference signal for the course angle and the sensors for measuring the output signals and equal to zero.

Figure 10. The controller outputs and when sensor signals are kept to zero and there is a unit step in the reference signal for the roll angle .

The system matrices of the discrete Kalman filter are declared as global real constants (type information given to MathCode C++).

Define the system functions (right-hand sides of the controller equations).

We perform a simulation of the controller equations using these right-hand sides with a specified initial condition.

FoldList can be used to compute an iteration, , that is, a discrete simulation.

Remove the last value.

Compute the output from the calculated input and state.

Plot the output with samples instead of time on the x axis.

Figure 11. The controller output when sensor signals are kept to zero and there is a unit step in the reference signal for the roll angle .

Create a loop that will be the main loop in the external code.

By construction, the functions f and g can be expanded to remove the dot-products and to reduce the computational effort. This is especially good when the system matrices contain many zeroes. The expansion is achieved using the EvaluateFunctions option.

Generate the code and compile it. The functions f and g will be expanded and subexpressions optimized before being code-generated.

We inspect the resulting C++ code.

Install the compiled code into Mathematica and test if the same simulation result as in Figure 11 is achieved.

Compare the controller outputs computed by Mathematica and by the external code. The maximum absolute error in the given sequence.

Plot the simulation when the compiled code is used.

We observe that the external simulation gives the same result as the simulation computed within Mathematica.

Uninstall the code and delete the temporary files.

Delete the temporary directory and all files it contains.