Modeling and LQR Control of Insect Sized Flap** Wing Robot

Abstract

Flying insects can perform rapid, sophisticated maneuvers like backflips, sharp banked turns, and in-flight collision recovery. To emulate these in aerial robots weighing less than a gram, known as flying insect robots (FIRs), a fast and responsive control system is essential. To date, these have largely been, at their core, elaborations of proportional-integral-derivative (PID)-type feedback control. Without exception, their gains have been painstakingly tuned by hand. Aggressive maneuvers have further required task-specific tuning. Optimal control has the potential to mitigate these issues, but has to date only been demonstrated using approxiate models and receding horizon controllers (RHC) that are too computationally demanding to be carried out onboard the robot. Here we used a more accurate stroke-averaged model of forces and torques to implement the first demonstration of optimal control on an FIR that is computationally efficient enough to be performed by a microprocessor carried onboard. We took force and torque measurements from a 150 mg FIR, the UW Robofly, using a custom-built sensitive force-torque sensor, and validated them using motion capture data in free flight. We demonstrated stable hovering (RMS error of about 4 cm) and trajectory tracking maneuvers at translational velocities up to 25 cm/s using an optimal linear quadratic regulator (LQR). These results were enabled by a more accurate model and lay the foundation for future work that uses our improved model and optimal controller in conjunction with recent advances in low-power receding horizon control to perform accurate aggressive maneuvers without iterative, task-specific tuning.

1 Introduction

Research in flap** wing insect-sized robots (FIRs) is motivated by their potential applications. These robots are small in size and are inexpensive to manufacture at a large scale, which makes them suitable for applications like detecting gas leaks, looking for survivors in disaster-prone areas, automated farm monitoring, running inspections on manufacturing lines, and weather monitoring. While still tethered and limited to operation inside a lab environment recent advances in tiny sensors and microcontrollers have brought them one step closer to achieving power [1] and sensor [2] autonomy.

Controlling FIRs presents significant challenges due to their highly nonlinear dynamics, manufacturing inconsistencies resulting in variability between robots, rapid wear and tear, and a high torque to moment of inertia ratio, approximately $10^{3}$ rad/sec², which leads to extremely fast dynamics.

Current state-of-the-art controllers for FIRs primarily utilize adaptive PID flight control systems for hovering [3]. Despite their widespread use, these controllers require substantial ad-hoc tuning and are task-specific, often failing to consider actuator, state, and environmental constraints. Maneuvering beyond basic linear responses, such as perching [4] and somersault [5] , requires a sliding mode controller combined with iterative learning of trajectory parameters. However, the parameters that were derived to perform an aggressive perch in [4] are specific to that task and cannot be applied to any other task.

Recent research has introduced optimal control strategies like modular Model Predictive Control (MPC) [6], which combines high-level MPC with a low-level controller for torque management, enabling operation beyond hovering. However, these systems have not been demonstrated on actual hardware for maneuvers beyond hovering. Additionally, data-driven MPC approaches like Tube-MPC [7] show promise for optimizing under actuator constraints and trajectory tracking for complex maneuvers such as ramps and infinity loops but remain too computationally intensive for implementation on sub-150 mg robots. Microprocessors small enough to be carried onboard, such as the 10 mg, 120 MHz STM32F4 used in the first wireless liftoff of an FIR, the UW Robofly in [1], are capable of floating-point math operations. Nevertheless, their performance will be limited to a fraction of desktop capabilities, just a few hundred MHz, for the foreseeable future.

Figure 1: RoboFly, an insect-sized flap** robot weighing 146 milligrams, hovers next to a flower using feedback from motion capture cameras. The robot performs this hovering maneuver using the LQR controller reported in this work.

Figure 2: Torque and Thrust Generation Mechanism in FIRs: (Top) Inspired by the work in [8], this figure shows that changing the signal parameters $\delta A$ and $V_{o}$ introduces roll and pitch torques, respectively. Here, $V_{bias}$ is the bias voltage. (Bottom) Map** of (a) thrust, (b) roll torque, and (c) pitch torque of the RoboFly used in this work. The thrust map** is obtained using a high-precision scale, while the torque map**s are obtained using the torque measurement device introduced in [8]. Pink dots represent the collected data points, and the green line represents the linear fit of the data. The corresponding equations for these linear fits are provided in Table 1.

The primary focus of the work in [9] is the precise tracking of the yaw angle, originally introduced in [10]. The PID controller used in [9] is tuned based on the robot’s dynamics. This enables the robot to hover and follow trajectories, such as an infinity loop. Our approach to controlling FIRs is based on the premise that an accurate model eliminates the need for laborious and unsatisfactory hand-tuning of PID gains. By framing the control problem within an optimal framework, we can design performance to maximize metrics like power efficiency or completion time. Using this model, we compute optimal gains around a fixed point for control. This was first demonstrated in [11], where the performance of PID and LQR controllers were compared on quadrotors with the target to stabilizing the orientation angles. The LQR controller demonstrated faster response in reaching the reference signal from high initial angles. The recent development of low-cost and accurate torque measurement device [8] has improved our ability to map the torque characteristics of these robots accurately. Furthermore, innovations such as pre-stacked actuators [12] and standardized manufacturing processes [13] have reduced variability in actuator performance and robot construction. In this work, we have made the following contributions:

1. For the first time, we developed and validated a stroke-averaged first-principle model by comparing it with high-speed trajectories collected from a sub-150 mg robot.

2. To the best of our knowledge, we present the first LQR implementation of controlled flight on an FIR.

This control strategy is computationally efficient, requiring a relatively small number of multiply-accumulate operations and trigonometric calculations per control step making it feasible for integration on tiny microcontrollers like the STM32F4, suitable for sub-150 mg robots. We expect our model and LQR controller will be able to serve as integral elements in a fast onboard receding horizon optimal controllers, such as those discussed in [14] and [15], that can optimize under actuator limits and state constraints. With these receding horizon controllers, the presented model can enable more aggressive maneuvers due to the robots’ high torque-to-inertia ratios, by scheduling the control gains for high translation speeds and attitude angles.

2 Results

Figure 3: Visualization of the collected data: The graph shows the robot achieving high attitude angles greater than 30^∘ and corresponding lateral/longitudinal speeds exceeding 0.4 m/s in the collected data. This highlights significant perturbations, which will be used to validate the stroke-averaged dynamics developed in this work. The color intensity on the graph represents the density of data points.

2.1 RoboFly

RoboFly (shown in Fig. 1) is a flap**-wing robot that weighs 150 mg. The robot features two piezoelectric actuators as muscles to flap its wings. These actuators are linked to a transmission mechanism that amplifies the actuators’ displacement of approximately 200 $\mu$m to a wing motion of about 60^∘. RoboFly has the capability to carry a payload up to 1.5 times its own weight and is powered via a wire-tether, which comprises four wires transmitting signals to operate the actuators.

RoboFly, like other piezo-actuated flap** wing robots [16][17], is operated by low-power 180 volt sinusoidal signals. It can generate roll torque, pitch torque, and thrust almost independently [18]. The analog voltage signal is generated using the equation,

$$V_{signal}=\frac{A+\delta A}{2}\sin\omega t+\frac{V_{bias}}{2}+\frac{V_{o}}{2}$$ (1)

Here, $V_{bias}$ is a constant bias signal voltage of 250 volt supplied to the top layer of the actuator. As shown in Fig. 2 (a), increasing the amplitude ($A$) of the sinusoidal signal increases the wing flap** amplitude, thereby generating greater thrust. Creating an amplitude differential ($\delta A$) between the wings, shown in Fig. 2 (b), increases thrust on one side while decreasing it on the other, which produces roll torque. Pitch torque (Fig. 2 (c)) is generated by adjusting the wing flap** either forward or backward relative to the robot’s body through a voltage offset ($V_{o}$) applied to the sinusoidal signal.

2.2 Theoretical Model

The dynamics of a flap** wing robot of the size of RoboFly can be defined by the same first principle model as a quadrotor [9]. Using the convention of $(X,Y,Z)$ as coordinates in an inertial frame and $(x_{b},y_{b},z_{b})$ as coordinates in the body frame, first-order differential equations of the body velocity $V_{b}=[u,v,w]^{T}$ of the robot is defined by

$$\dot{u}= g\sin\theta+f_{a_{1}}-(qw-rv)$$ (2)

$$\dot{v}= -g\cos\theta\sin\phi+f_{a_{2}}-(rv-pw)$$ (3)

$$\dot{w}= -g\cos\theta\cos\phi+f_{a_{3}}-(pv-qu)+\frac{\Gamma}{m+m_{M}}$$ (4)

Here, $\omega_{b}=[p,q,r]^{T}$ is the body angular velocity, $g$ is the acceleration due to gravity, $m$ is the mass of the robot and, $m_{M}$ is the mass of the MoCap markers. Rotation is defined by the widely used 321 rotation matrix. The 321 rotation follows the yaw ($\psi$) $\rightarrow$ pitch ($\theta$) $\rightarrow$ roll ($\phi$) sequence. $\Gamma$ is the thrust applied to the robot in the positive $z_{b}$ direction. $F_{a}=[f_{a_{1}},f_{a_{2}},f_{a_{3}}]^{T}$ is the unmodeled dynamic force, which includes stroke averaged aerodynamic force in body frame caused by the drag due to flap** wings. Similarly, rotational dynamics can be defined by the first-order differential equation in body angular velocity ($\omega_{b}=[p,q,r]^{T}$).

$$\dot{p}= L+\frac{\tau_{r}}{J_{xx}}-\frac{J_{zz}-J_{yy}}{J_{xx}}qr$$ (5)

$$\dot{q}= M+\frac{\tau_{p}}{J_{yy}}-\frac{J_{xx}-J_{zz}}{J_{yy}}rp$$ (6)

$$\dot{r}= N-\frac{J_{yy}-J_{xx}}{J_{zz}}pq$$ (7)

Here, $J=diag([J_{xx},J_{yy},J_{zz}])$ is the diagonal moment of inertia matrix of the robot. $\tau_{c}=[\tau_{r},\tau_{p},0]^{T}$ is defined as the input torque vector comprising of roll torque ($\tau_{r}$) about $x_{b}$ and pitch torque ($\tau_{p}$) about $y_{b}$. $\tau_{a}=[L,M,N]^{T}$ is the unmodeled dynamic moment, which also includes averaged aerodynamic torque caused by the drag due to flap** wings.

2.2.1 Model Parameters

We precisely measured the robot’s parameters using a sensitive, custom-build force torque sensor. Results are tabulated in Table 1. The mass of the robot was determined using a high-precision balance with a resolution of 0.1 mg. To estimate the robot’s moment of inertia, we measured the mass of its various components, including the piezoelectric actuators, wings, airframes, and motion capture markers. These measurements were then input into a CAD model to calculate the robot’s moment of inertia matrix. The high-precision scale also facilitated in the calculation of thrust map**, where the thrust generated by the robot at specific flap** amplitudes ($A$) was recorded. During the experiment, the wings were flapped for a period of 1 sec at the constant frequency of 180 hz. The scale can take accurate stroke averaged measurements of thrust. The test setup also makes sure that the robot is away from any surrounding objects to avoid ground effects. We employed a least squares fit method to model the thrust based on this data (Fig. 2-left). The learned linear fit of the thrust map** is shown in Table 1. Given that the lifetime of these robots is about 10 minutes [19], we aimed to minimize the total operating time to prevent mechanical fatigue. Therefore, for the map** of thrust and torques, we took only two or three measurements to establish a trendline that can be used in the model.

To measure the torque response to different voltage inputs, that is, torque map**, we used a device similar to the one in [18]. By applying inputs $V_{o}$ and $\delta V$, we generate roll and pitch torques, respectively. These torques induced angular deflections on the device, which are linearly correlated with the applied torques, which was observed in practice in [18]. These deflections were accurately measured using the motion capture system, allowing us to map torques effectively (Fig. 2-middle and right). This comprehensive approach to parameter measurement ensures a robust foundation for our model.

2.3 Trajectory Data

We collected 8 seconds of trajectory data from 7 separate flights with wings flap** at a frequency of 180 Hz, controlled by a PID flight controller [13]. To capture flight perturbations, we set the desired points away from the robot’s initial position, focusing on collecting more data with perturbations in lateral, longitudinal, and vertical dynamics. To avoid capturing too much stable hovering data, most flight trajectories were shortened to the duration required for the robot to reach the set positions. The robot was equipped with four retro-reflective markers to track its position and orientation through a motion capture system comprised of four Prime 13 cameras by OptiTrak, Inc., Salem, OR. Position and quaternions from the motion capture system running at 240 hz were used to calculate $V_{b}$, $\omega_{b}$, and Euler angles of the robot offline.

2.3.1 Body Offset

A critical difference between the robot’s trajectory data and the modeled dynamics from equations 2-7 can stem from the misalignment between the body z-axis, as defined in the motion capture software, and the robot’s thrust vector. The dynamics, detailed in equations 2-4, assume that the thrust vector is perfectly aligned with the body z-axis, an assumption that may not hold in practice. This misalignment issue occurs because the thrust vector’s direction is not known at the time that the robot’s body coordinates are defined in the motion capture software. A tilted thrust vector introduces lateral and longitudinal forces. To reduce this error, we redefined the body coordinate system after performing a short 0.3 s uncontrolled trimmed flight. Since during a trimmed flight, the robot takes off approximately vertically, so its trajectory can be used to estimate the direction of the thrust vector and therefore align the z-axis of the body coordinate system to that direction. This, however, is based on the assumption that the body coordinate system is not redefined in between the experiments and remains the same throughout the data collection and control process.

Parameter	Symbol	Value
Mass of the robot	$m$	$150\times 10^{-6}$ kg
Mass of the MoCap markers	$m_{M}$	$36\times 10^{-6}$ kg
Moment of inertia	$J$	diag([$3.12\times 10^{-9}$, $2.97\times 10^{-9}$, $0.55\times 10^{-9}$]) kg.m2
Thrust	$\Gamma$	$3.27\times 10^{-5}A-0.0024$ N
Roll Torque	$\tau_{r}$	$0.48\times 10^{-6}\delta A$ Nm
Pitch Torque	$\tau_{p}$	$0.11\times 10^{-6}V_{o}$ Nm

Table 1: Measured parameters of RoboFly. Calculated Moment of inertia $J$ is a diagonal matrix of moment of inertia about the principal axes. Map** from $A$ to $\Gamma$, $\delta A$ to $\tau_{r}$, and $V_{o}$ to $\tau_{p}$ are the equations of linear fit from Fig 2 (a),(b) and (c) respectively.

2.3.2 Visualization of Collected Data

Unlike traditional aircraft, where the flight envelope is defined by changes in velocity against variations in angle of attack, flap** wing robots exhibit continuous angle of attack variations throughout the flap** cycle. Therefore, we determined that representing the flight state in terms of angular orientation and velocity would be more pertinent characterization. Characterizing in this way elucidates the robot’s ability to maintain controlled flight across different tilt angles and the associated longitudinal/lateral speeds at these angles, which are critical for maneuverability. A robot that can sustain higher tilt angles and translational speeds in controlled flight is indicative of superior maneuverability, enabling it to execute tighter turns. As shown in Fig. 3, based on data collected from the flight trajectories of the RoboFly, the robot is able to get to attitude angles of approximately 30^∘ and body velocity of around 0.4 m/sec.

2.4 Controller Implementation

In our experiments, we did not control the yaw rotation of the robot. Hence the LQR was designed to optimize the dynamics in the body coordinate system, which are independent of the yaw rotation. The state vector of the robot is defined by $\sigma=[d_{x},d_{y},d_{z},u,v,w,\phi,\theta,p,q]^{T}$. Here, $d=[d_{x},d_{y},d_{z}]^{T}$ is the position in body coordinates and $V_{b}=\dot{d}=[u,v,w]$. The controller assumes that the angular velocity about $z_{b}$, denoted as $r$, which arises from manufacturing uncertainties, remains constant. Its value affects fictitious forces and torques in body coordinates that appear in equations 2-7. The controller calculates the inputs $u^{*}=[A,\delta A,V_{o}]$, which directly controls the acceleration in $z_{b}$, torque about $x_{b}$, and torque about $y_{b}$ axis. The $Q$ matrix used in our experiments is a diagonal matrix, $Q=diag([0.02,0.02,0.01,0.1,0.1,0.1,1,1,4,4])$; $R$ is also a diagonal matrix, $R=diag([2,1,1])$. The ratio of $Q$ and $R$ matrix used here was obtained with the knowledge of our model and only one experimental flight. The feedback loop includes a motion capture system that provides state feedback in terms of the robot’s position and orientation (expressed as quaternions). This data feeds into a Simulink real-time system in which the body angular velocities, euler angles, and velocity in world frame are calculated. The controller receives the desired position in the world coordinates. Finally, the resulting error in the world coordinates is converted to the body coordinates and is multiplied by the pre-calculated LQR gain matrix $k$ to determine the control inputs, $u=-k(\sigma_{des}-\sigma)$, for the robot. Control loop used for our experiments is shown in Fig. 5(a).

Figure 4: Model validation plots: Measured accelerations (green) from the RoboFly trajectories plotted with the predicted accelerations (pink) calculated using the theoretical model.

Figure 5: LQR control loop and hovering trajectories: (a) The LQR control loop used to perform hovering and trajectory tracking maneuvers. Here, R represents the 3-2-1 rotation matrix. (b) The plot displays five different hovering trajectories, showing the robot maintaining a stable attitude and remaining close to the starting position. The mean RMS error and standard deviation for the trajectories are 4.17$\pm$ 0.37 cm.

Figure 6: Results of LQR implementation on the RoboFly: (a) Response to the external disturbance applied via kevlar thread is shown in the photo composite. The robot does a recovery maneuver to get back to the stable attitude and ultimately flies to the desired position.(b) The photo composite shows the RoboFly tracking a 10 cm radius circular trajectory over a 4.5-second flight using a constant pre-calculated LQR gain. (c) The desired trajectory, depicted in green, was provided to the controller in the form of position and velocity set points while magenta is the actual trajectory followed by the robot. The RMS error for this maneuver for x-y position tracking is 2.8 cm. The video link of the experiments is shown in the abstract page.

2.5 Model Validation

For simplicity, the model we used in this work excludes dam** and drag components, thus setting the force and moment vectors $[f_{a_{1}},f_{a_{2}},f_{a_{3}}]^{T}$ and $[L,M,N]^{T}$ to zero in equations (2-7). We validated the theoretical model accelerations by comparing them with measured acceleration data from flight tests, as depicted in Fig. 4. This comparison includes seven separate flight trajectories stacked in time.

The model can predict translational accelerations in the body coordinate system, represented by $[\dot{u},\dot{v},\dot{w}]^{T}$, with the root mean squared ($L^{2}$) error of 53.4 m/sec², 56.9 m/sec², and 36.7 m/sec², respectively. The errors in rotational accelerations, $[\dot{p},\dot{q}]^{T}$, are 9.2e³ rad/sec² and 7.6e³ rad/sec², respectively. The more substantial errors in the rotational domain can largely be attributed to the aforementioned aspect of small flight vehicles that their angular accelerations are large.

We believe, and our results show, that the model is still adequate for controller design purposes. This is based on two key considerations: Firstly, the actuation delay for the rotational system is minimal, as rotational acceleration occurs almost instantaneously once torque is applied. Secondly, the robot will eventually have a gyroscope onboard which is capable of providing rapid rotational velocity feedback at 1 to 16 kHz, which will significantly enhance the ability to perform rapid feedback corrections. Thus, even with its limitations, this model provides a sufficient foundation for develo** an effective controller.

2.6 Hovering

The initial task was to hover around a desired position. We did five such flights, each lasting 2 seconds, and the robot managed to hover using the pre-calculated LQR gain, with RMS errors of 4.2 cm, 3.8 cm, 4.05 cm, 4.03 cm, and 4.8 cm. For hovering tasks, a PID controller, as referenced in [16] and [20], outperforms this with an average RMS error of 2 cm. We think this is due to the fact that PID controllers are manually tuned for specific tasks, whereas the LQR controller used here was only tuned once for determining the Q and R matrices and is more general, as it can be linearized about different states. Five hover trajectories are shown in Fig. 5(b).

2.7 Response to External Disturbance

To prevent crashes during our experiments, we suspend the robot using a lightweight Kevlar thread. There is slack in the Kevlar thread in all our experiment videos, which demonstrates that the thread does not exert any force on the robot. However, in this particular experiment, we intentionally applied force by pulling the robot with the Kevlar thread to test the controller’s response to external disturbances. This causes the accelerations $>$2.5g. As show in Fig. 6(a), the robot was able to stabilize itself and flies towards the desired position.

2.8 Trajectory Tracking

Here the robot was asked to follow a pre-computed circular trajectory of 10 cm radius. The robot was able to follow the given trajectory in a 4.5-second flight with an RMS error of 2.8 cm in x-y position tracking. Photo composite of the maneuver is shown in Fig. 6(b). The desired waypoints on the trajectory were given in the form of position and velocity set points. The controller used the same Q and R matrices as the hovering maneuver. For comparison, in [7] authors tracked a 5 cm radius circular trajectory with the reported x-y position error of 1.8 cm. Our greater position tracking error is likely due to a much higher flight velocity (25 cm$/$sec vs a maximum speed of 5.2 cm$/$s in [7]) and a larger circular trajectory, which would result in larger disturbances by the wire tether.

3 Discussion

In our study, we developed and validated a theoretical model of the UW RoboFly. Utilizing this model, we successfully implemented an infinite horizon Linear Quadratic Regulator (LQR) control strategy. This enabled us to achieve stable hovering, recovery maneuver, and trajectory tracking using the pre-calculated LQR gain. While its RMS position error was higher than other recent reports, it was following a much faster trajectory. Notably, our controller works with minimal computational demands, making it ideal for integration into microcontrollers suited for tiny robotic platforms.

3.1 Limitations and Future Work

This work provides a foundation for some important next steps. First, the single LQR gain could be replaced with one that is “gain-scheduled” for linearizations about different states, such as forward flight or flight into the wind. This would also impose a minimal computational load on any conceivable microcontroller. Second, collecting data over a broader flight envelope could improve model identification and therefore flight control. Third, our current controller does not perform any sort of adaptivity. Future models could account for the offset between the thrust vector and the body-z axis and estimate the drag model using the discrepancy between expected and actual translational velocity as input[16, 20]. Such refinements are expected to yield even lower RMS errors for specific tasks and potentially enable more agile maneuvers.

While gain scheduling has the potential to broaden the envelope in which the LQR controller operates well, it does not take into account an important aspect of FIRs. This is that they are subject to constraints on the magnitude of outputs that the actuators can produce. A more advanced control technique, known as Receding Horizon Control (RHC) or Model Predictive Control (MPC), can accurately factor those into the optimization process. Although [7] applied this technique, their controllers were too demanding for a microcontroller. We expect that the advances that were used in Tiny-MPC [14] to perform RHC on the 120 MHz microcontroller onboard the the 30 g Crazyflie helicopter could be adapted to the different dynamics and constraints of the Robofly. By taking into account actuator constraints and any improved system characterization, more precise and more aggressive agile maneuvers should be possible without hand-tuning.

4 Methods

4.1 Infinite Horizon LQR

Infinite horizon LQR controller [21] optimizes the quadratic cost function subject to linearized dynamics constraints. In this work we use the continuous time formulation of LQR.

$$J= \int_{0}^{\infty}\sigma(t)^{T}Q\sigma(t)+u(t)^{T}Ru(t)$$ (8)

$$\text{subject to }\dot{\sigma}(t)= A_{d}\sigma(t)+B_{d}u(t)$$ (9)

Here, $\sigma(t)$ is the state vector of the robot at time $t$, $A_{d}$ and $B_{d}$ are the linearized dynamics matrices. If the system is completely stabilizable, we can write this optimal control problem in terms of the Hamiltonian function, which incorporates both the system dynamics and the cost function. The Hamiltonian for the LQR problem is given by $H(x,u,\lambda)=\sigma^{T}Q\sigma+u^{T}Ru+\lambda^{T}(A_{d}\sigma+B_{d}u)$. $\lambda$ is the costate vector, $Q$ and $R$ are positive semi-definite and positive definite matrices respectively. By taking the derivative of Hamiltonian with respect to $\sigma$, $u$, and $\lambda$ and setting it to zero we get the closed loop dynamics as,

$$\begin{bmatrix}\dot{\sigma}\\ \dot{\lambda}\end{bmatrix}=\begin{bmatrix}A&-BR^{-1}B^{T}\\ -Q&-A^{T}\\ \end{bmatrix}\begin{bmatrix}\sigma\\ \lambda\end{bmatrix}$$ (10)

The steady state solution of the Riccati equation can be described in terms of the eigenvectors of the Hamiltonian matrix in the closed loop dynamic equation 10 associated with the negative real part eigen values. These negative real part eigenvalues of the matrix are also the eigenvalues of the closed-loop matrix $A-BR^{-1}BP$. The feedback gain $k=-R^{-1}B^{\prime}P$ can be obtained using $P$ as the Schur form of the closed loop dynamics matrix such that:

$$u(t)= -k(\sigma_{des}(t)-\sigma(t))$$ (11)

Here $\sigma_{des}(t)$ is the desired state of the robot at time t.

Acknowledgements We would like to thank Y. M. Chukewad for insightful discussions about the Robofly.