Calculate time from one state to another using 6DOF state space modeling of a quadcopter

Question

I'm new here. I have the 6DOF state space representation of a quadcopter given below,
\begin{align} \dot x(t) &= Ax(t) + Bu(t) \end{align}
Where, $x(t)$ = State Vector, $u(t)$ = Input (or control) Vector, A = System Matrix, B = Input\control Matrix.
Now for the 6DOF modeling, I have the state vectors,
$x$ = $[x$ $y$ $z$ $x'$ $y'$ $z'$ $\phi$ $\theta$ $\psi$ $\phi'$ $\theta'$ $\psi']^T$
$x'$ = $[x'$ $y'$ $z'$ $x''$ $y''$ $z''$ $\phi'$ $\theta'$ $\psi'$ $\phi''$ $\theta''$ $\psi'']^T$
Where,
Position along x axis = $x$, Position along y axis = $y$, Position along z axis (height) = $z$, Velocity along x axis = $x'$, Velocity along y axis = $y'$, Velocity along z axis = $z'$, Roll angle = $\phi$, Pitch angle = $\theta$, Yaw angle = $\psi$, Roll rate = $\phi'$, Pitch rate = $\theta'$, Yaw rate = $\psi'$.
And the input vector,
$u$ = $[U_1$ $U_2$ $U_3$ $U_4]^T$
Where,
$U_1$ = Total Upward Force = $F - mg$,
$U_2$ = Pitch Torque along x axis,
$U_3$ = Roll Torque along y axis,
$U_4$ = Yaw Torque along z axis.
From the above equations the state differential equations can be obtained from,
$x'=x'$, $y'=y'$, $z' = z'$, $x'' = -g\theta$, $y'' = g\phi$, $z'' = -U_1/m$, $\phi' = \phi'$, $\theta' = \theta'$, $\psi' = \psi'$, $\phi'' = U_2/l_x$, $\theta'' = U_3/l_y$, $\psi'' = U_4/l_z.$
Now we can write the model as a linear system,
$\dot x = Ax + Bu$
Where,
\begin{bmatrix} x'\\ y'\\ z'\\ x''\\ y''\\ z''\\ \phi'\\ \theta'\\ \psi'\\ \phi''\\ \theta''\\ \psi'' \end{bmatrix}=\begin{bmatrix} 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & -g & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & g & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}
\begin{bmatrix} x\\ y\\ z\\ x'\\ y'\\ z'\\ \phi\\ \theta\\ \psi\\ \phi'\\ \theta'\\ \psi' \end{bmatrix} + \begin{bmatrix} 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ -\frac{1}{m} & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & \frac{l}{l_x} & 0 & 0\\ 0 & 0 & \frac{l}{l_y} & 0\\ 0 & 0 & 0 & \frac{l}{l_z} \end{bmatrix} \begin{bmatrix} U_1\\ U_2\\ U_3\\ U_4\\ \end{bmatrix}
Image of the matrices for better understanding

Now I want to calculate the time required to go from one state, lets say $x_1$ = [1 5 6 4 4 8 30 45 20 6 11 15]^T to another state $x_2$ = [10 500 63 41 45 82 40 55 60 6 11 18]^T. How can I do it? assuming that I know the value of the four inputs and all other values. Or do I need any other modeling? I apologize if my question does not make any sense. Please help me regarding this issue.

Answer 1

Mathematicians like to use matrix notation because it can turn a huge mess of equations into single tidy one. It also can be exploited to reduce the degree of an ODE to one. This is really helpful when you are proving existence and regularity theorems.

But in many, many cases when you are actually calculating, it turns a fairly simple system into something vastly more complicated. That is certainly the case here.

Since you represent their combinations as $u(t)$, presumably the $U_i$ are all time-dependent. But I will assume that $l_x, l_y, l_z, m$ are not (I.e., I am assuming an electric quadcopter, where fuel is not being burned.). Your system of equations has the obvious solution $$\phi(t) = \phi'(0)t + \phi(0)+\frac1{l_x}\int_0^t\int_0^\tau U_2(s)\,dsd\tau\\ \theta(t) = \theta'(0)t + \theta(0) + \frac1{l_y}\int_0^t\int_0^\tau U_3(s)\,dsd\tau\\ \psi(t) = \psi'(0)t + \psi(0) + \frac1{l_z}\int_0^t\int_0^\tau U_4(s)\,dsd\tau\\ z = z(0) + z'(0)t -\frac 1m\int_0^t\int_0^\tau U_1(s)\,dsd\tau\\ y = y(0) + y'(0)t + g\int_0^t\int_0^\tau \phi(s)\,dsd\tau\\ x = x(0) + x'(0)t - g\int_0^t\int_0^\tau \theta(s)\,dsd\tau$$

Where the various values at $t = 0$ are the corresponding elements of your $x_1$ starting state. The derivative functions can be found by differentiating each equation, so I didn't give them. This cannot be simplified further without knowing what the $U_i$ are.

For the problem of just finding the time difference between $x_1$ and $x_2$, you might be able to solve it just using one of the equations. If one of $U_1, U_2, U_3, U_4$ is always $\ge 0$, the corresponding function $\phi, \theta, \psi$ or $z$ will be increasing. Thus there will be a single $t$ that makes the function take on its $x_2$ value. Assuming the full state path actually does pass through $x_2$, this has to be the time where it does. So it gives you the time interval.

When the $U_i$ all switch between negative and positive values, you will need to find each solution to one of the equations, say $z(t) = z_2$, then check that solution against the other equations. If it gives the correct answer to all $12$ values in $x_2$, it is the time you are after. Otherwise, find another solution to your first equation, and keep looking.