Fault Recovery of a UAV using MATLAB

Question

I'm currently investigating fault recovery in a UAV using MATLAB.

I've been given several variables:

• phi = the roll angle
• psi = the yaw/heading angle
• beta = the side slip
• p = roll rate
• r = yaw/heading rate
• delta_r = angle of the rudder
• delta_a = angle of the aileron (the aileron is currently inactive).

My input is the rudder angle.

I've simulated a step fault in the yaw/heading sensor and I'm trying to recover from it.

To detect that there was a fault in the heading sensors: I compared the actual side-slip with the modeled side slip. If there is a difference between the "real' system and the model in heading angle, but no difference in side-slip, then the heading sensor is faulty.

To recover from this fault: I want to reconfigure the side-slip so I can use it to determine the heading/yaw angle. In order to do this I need to multiply the side-slip by the transfer function between side-slip and heading/yaw angle.

We've been given the following equations:

I'm having some trouble coming up with a transfer function between side slip and heading angle.

I figured the best thing to do, as I am using a simulation filled with arrays, is to turn all the derivatives into their discrete equivalent - of course this is means I'm not using a transfer function, but an equation. Taking the equation for the derivative of beta and discretizing it...

When I plug this in, the system doesn't recover.

I even tried doing Laplace transforms, but I get stuck here:

And I'm not sure how I could apply that in MATLAB

Is there a mistake in my logic or approach? How would I turn the discrete system into a transfer function?

Answer 1

Find the transfer function $\delta_r(t)\to \psi (t)$ and $\delta _r(t)\to \beta (t)$. Divide the first transfer function by the second to get the transfer function $\beta (t)\to \psi (t)$.

These calculations can be tedious. I used Mathematica to get the following result.

ssm = StateSpaceModel[{p'[t] == -11.450 p[t] + 2.7185 r[t] - 
      19.4399 \[Beta][t] + 78.4002 Subscript[\[Delta], a][t] - 
      2.7282 Subscript[\[Delta], r][t], 
    r'[t] == 
     0.5068 p[t] - 2.9875 r[t] + 23.3434 \[Beta][t] - 
      3.469 Subscript[\[Delta], a][t] + 
      13.9685 Subscript[\[Delta], r][t], \[Beta]'[t] == 
     0.0922 p[t] - 0.9957 r[t] - 0.4680 \[Beta][t] + 
      0.3256 \[Phi][t], \[Phi]'[t] == 
     p[t] + 0.0926 r[t], \[Psi]'[t] == r[t]}, {p[t], 
    r[t], \[Beta][t], \[Phi][t], \[Psi][t]}, {Subscript[\[Delta], r][
     t]}, {\[Psi][t], \[Beta][t]}, t];
tfm = TransferFunctionModel[
  Simplify[Divide @@ Flatten[TransferFunctionModel[ssm, s][s]]], s]

$$\frac{0.986478 s^3+11.6592 s^2+6.59389 s+4.77963}{-1. s^3-10.9882 s^2+1.02337 s}$$

The transfer function is unstable (assuming I did not make any typos).

TransferFunctionPoles[tfm]

{{{-11.0805, 0, 0.0923575}}}

However, the transfer function from $\psi (t)\to \beta (t)$ is stable

$$ \frac{-1.01371 s^3-11.1388 s^2+1.0374 s}{s^3+11.819 s^2+6.68427 s+4.84515}$$

and we can plot its step response.

Plot[Evaluate@OutputResponse[tfm, UnitStep[t], {t, 0, 20}], {t, 0, 20}, PlotRange -> All]

Answer 2

Why dont you construct a state-space model from the given equations (which happen to be in the appropriate form) and either continue using the state space model or convert it to a transfer function in matlab.

A continuous-time state-space model looks like this: $$\dot{x} = Ax + Bu$$ $$y = Cx+Du$$ Where $x$ and $u$ are vectors and $A$ and $B$ matrices. $$\begin{bmatrix}\dot{x}_1 \\ \dot{x}_2 \\ \vdots \\ \dot{x}_n\end{bmatrix} = A\begin{bmatrix}x_1 \\ x_2 \\ \vdots \\ x_n\end{bmatrix} + B\begin{bmatrix}u_1 \\ u_2 \\ \vdots \\ u_m\end{bmatrix}$$

So, first determine your inputs, these are external inputs. Usually the ones you control, in your case $\delta_r$ and $\delta_a$. The states of the system (denoted with $x$) represent the system dynamics. A state-space equation always expresses a single time-derivative as a linear sum of the states and inputs. In your case these are: $p, \phi, \psi, \beta$ and $r$. The output of the system, denoted with $y$ represents the things you measure. If you measure every state, $C$ is just an identity matrix. $D$ is a feed through matrix and is usually zero. So if you have constructed the $A, B, C$ and $D$ matrix, you can use the MATLAB command sys = ss(A,B,C,D) to construct the system. Although this system object is different to that constructed using tf(den, num), it can be used just like one.

To convert the system to discrete time, use sysD = c2d(sys, Ts, 'zoh');. Here 'zoh' is the discretization method (zero-order hold), c2d supports multiple methods.

To convert the system to a transfer function matrix, use sysTF = ss2tf(A,B,C,D);.

EDIT: To answer your question, you must modify the state-space matrix such that $\beta$ is an input. While this might sound easy, it requires quite some rewriting. What I would attempt is to merge 2 states so one state becomes $\beta + \alpha r$. Where $\alpha$ is some constant. Its derivative will simply be $\frac{d\beta}{dt} + \alpha \frac{dr}{dt}$. To ''fix'' the error caused by this change, you add $\beta$ as a new input. So the state equation of $\psi$ becomes: $$\frac{d\psi}{dt} = \frac{1}{\alpha}(\beta + \alpha r) - \frac{1}{\alpha}\beta$$

As $\beta$ is now an input, the transfer function can be computed. For simplicity I'd suggest you make $C$ such that $\psi$ is the only output. The resulting transferfunction matrix will look something like this: $$H(s) = \begin{bmatrix}\frac{\psi(s)}{\beta(s)} & \frac{\psi(s)}{\delta_a(s)} & \frac{\psi(s)}{\delta_r(s)}\end{bmatrix} $$ Now to get the transfer function from $\beta$ to $\psi$ you just have to pick the first matrix. Remember that you must describe $\beta$ as your first input in the state-space model. Otherwise it is in a different place.

Good luck!