diff --git a/docs/assets/estimator_assets/estimator_cmake.png b/docs/assets/estimator_assets/estimator_cmake.png new file mode 100644 index 0000000..4e9e0f3 Binary files /dev/null and b/docs/assets/estimator_assets/estimator_cmake.png differ diff --git a/docs/assets/estimator_assets/estimator_full_ros_interactions.png b/docs/assets/estimator_assets/estimator_full_ros_interactions.png new file mode 100644 index 0000000..6ce4118 Binary files /dev/null and b/docs/assets/estimator_assets/estimator_full_ros_interactions.png differ diff --git a/docs/assets/estimator_assets/estimator_override_function.png b/docs/assets/estimator_assets/estimator_override_function.png new file mode 100644 index 0000000..b3ffd1c Binary files /dev/null and b/docs/assets/estimator_assets/estimator_override_function.png differ diff --git a/docs/assets/estimator_assets/estimator_ros_input_output.png b/docs/assets/estimator_assets/estimator_ros_input_output.png new file mode 100644 index 0000000..a31629a Binary files /dev/null and b/docs/assets/estimator_assets/estimator_ros_input_output.png differ diff --git a/docs/developer-guide/rosplane/controller/controller-general-overview.md b/docs/developer-guide/rosplane/controller/controller-general-overview.md index 6d8f607..d8c2210 100644 --- a/docs/developer-guide/rosplane/controller/controller-general-overview.md +++ b/docs/developer-guide/rosplane/controller/controller-general-overview.md @@ -83,7 +83,7 @@ A table of arguments and parameter files that work out of the box is given in th !!! note Filepaths will need to be altered to work. !!! note - The most common way of running the controller is in through a launch file with the rest of the ROSplane pipeline running as well. + The most common way of running the controller is through a launch file with the rest of the ROSplane pipeline running as well. See the ROSplane Overview in the Developer and User Guides for more details. diff --git a/docs/developer-guide/rosplane/estimator/estimator-base.md b/docs/developer-guide/rosplane/estimator/estimator-base.md new file mode 100644 index 0000000..b7a88df --- /dev/null +++ b/docs/developer-guide/rosplane/estimator/estimator-base.md @@ -0,0 +1,49 @@ +# Estimator Base + +## Overview + +The estimator base implements the basic ROS interfaces for the estimator. +This includes setting up subscribers, publishers and initializing parameter management. +The idea of the base class, is that all interfacing with ROS and shared resources across all inheritance levels happens or are contained in this class. + +## ROS Interfaces + +The estimator has the following ROS interfaces. + +| ![Diagram of Estimator ROS Sensors](../../../assets/estimator_assets/estimator_ros_input_output.png "Estimator ROS Sensors") | +|:--:| +|*Figure 1: Estimator's ROS interactions.*| + +The estimator has several ROS interfaces that are tracked as member variables of the `estimator_base` class. +They are summarized in the table below: + +| ROS Interface | Topic | Explanation | Message Type | +|:------:|:-------:| :---: | :---: | +|
`vehicle_state_pub_`
| `/estimated_state` | Publishes the estimated state of the vehicle. | State.msg | +|
`gnss_fix_sub_`
| `/navsat_compat/fix` | Subcribes to the GNSS position information. | NavSatFix.msg | +|
`gnss_vel_sub_`
| `/navsat_compat/vel` | Subcribes to the GNSS velocity information. | TwistStamped.msg | +|
`imu_sub_`
| `/imu/data` | Subcribes to the IMU data (both Gyro and Accel). | Imu.msg | +|
`baro_sub_`
| `/baro` | Subcribes to the barometer pressure information. | Barometer.msg | +|
`airspeed_sub_`
| `/airspeed` | Subcribes to the differential pressure information. | Airspeed.msg | +|
`status_sub_`
| `/status` | Subcribes to the aircraft status information. | Status.msg | + +!!! note + The NavSatFix, TwistStamped and Imu messages are from standard ROS message packages. Barometer, Airspeed and Status messages are from `rosflight_msgs`. + +## Parameters + +| **Parameter** | **Explanation** | **Type** | **Range** | +| :---: | :---: | :---: | :---: | +| `rho` | The density of the air. | double | ~1.225 $\frac{kg}{m^3}$ | +| `gravity` | The acceleration due to gravity. | double | ~9.81 $\frac{m}{s^2}$ | +| `estimator_update_frequency` | The frequency that the estimator will run estimations. | double | $\geq 100$ Hz | +| `gps_ground_speed_threshold` | This determines when the aircraft is moving fast enough to get a course estimate. | double | $\geq$ 0.3 $\frac{m}{s}$ | +| `baro_measurement_gate` | The maximum allowable instantaneous change in barometer pressure measurement. | double | ~1 meter | +| `airspeed_measurement_gate` | The maximum allowable instantaneous change in differential pressure measurement. | double | ~5 $\frac{m}{s}$ | +| `baro_calibration_count` | The number of barometer measurements used to do calibration. | int | $\geq 100$ | + +## Modifying the Estimator + +Update when full state estimator and continuous-discrete are both implemented and a swap between them works. + + diff --git a/docs/developer-guide/rosplane/estimator/estimator-example.md b/docs/developer-guide/rosplane/estimator/estimator-example.md new file mode 100644 index 0000000..5ba15c2 --- /dev/null +++ b/docs/developer-guide/rosplane/estimator/estimator-example.md @@ -0,0 +1,387 @@ +# Estimator Example + + + +## Overview + +The `estimator_example` class implements a continuous-discrete Kalman filter as described in section 8.5 of the [UAV book](https://github.com/randybeard/mavsim_public) or 8.6 and 8.7 of volume one of the same book. +Specifically, this estimator uses algorithm 2 of chapter 8 in Volume 1. +It utilizes a two stage estimation process along with low pass filtering the inversion of a few sensor models and direct measurements. +The roll and pitch of the aircraft are estimated first. +This is called the attitude estimation step though not all of the attitude is estimated here. +The other states are then estimated as a all at once. +This is called the position estimation step, though more than just position is estimated during this step. +The estimator runs on a set timer with a configurable frequency (see Parameters section for details). + +The estimator makes large use of something called a pseudo-measurement. +These measures assume that the side slip angle is zero and the corresponding wind triangle solution. +This allows us to take these measures and then find the wind in the north and east directions along with the yaw of the aircraft. +They are more fully explored in section 8.11.4 of the UAV book. + +## Nomenclature + +| Symbol | Meaning | Range | +|:------:|:-------:| :---: | +|$\large{\chi}$| Course | $[-\pi,\pi)$ | +|$\large{\phi}$| Roll | $[-\pi,\pi)$ | +|$\large{\theta}$| Theta | $[-\pi,\pi)$ | +|$\large{\psi}$| Yaw | $[-\pi,\pi)$ | +|$\large{h}$| Altitude | - | +|$\large{p}$| Roll Rate | - | +|$\large{q}$| Pitch Rate | - | +|$\large{r}$| Yaw Rate | - | +|$\large{V_a}$| Airspeed | $\geq 0$ | +|$\large{w_n}$| Wind North | - | +|$\large{w_e}$| Wind East | - | + +## Sensor Model Inversion + +The roll, pitch and yaw rates are directly measured by the rate gyro and then low pass filtered. +The low pass filter is a simple alpha filter described by: + +$$ + a_{n} = a_{n-1} \alpha + (1 - \alpha) a_{measured} +$$ + +Where $a$ is the state. +This filter technique is used on the measurements before they are used in an estimate throughout the estimator. + +The following are calculated directly from a model of the sensor: + +
+ +| State | Measured Value | Model Equation | Inversion Equation | +|:--:|:--:|:--:|:--:| +| $h$ - altitude | $P$ - absolute pressure (Pa) | $P = \rho_{air} gh$ | $h = \frac{\rho_{air} g}{P}$| +| $V_a$ - airspeed | $\Delta P$ - differential pressure (Pa) | $\Delta P = \frac{1}{2} \rho_{air} V_a^2$ | $V_a = \sqrt{\frac{2}{\rho_{air} g} \Delta P}$| + +
+ +These values are then used in the first step of the estimator. + +## Attitude Estimation + +The first step in estimating the attitude is understanding how we propagate our guess between measurements. +Our estimates are on roll angle, $\phi$, and on pitch angle, $\theta$. +These estimates are part of the state $\hat{x}_a$, where the hat over the variable indicates an estimate. + +### Propagation + +At each call of the estimation algorithm, the estimate from the previous time step is propagated to the next time step. +The propagation step is broken up into $N$ smaller steps to yield an estimate for the current time step. +$N$ is typically 10, meaning that the previous estimate that was updated by a measurement is updated in 10 steps instead of a single step to calculate the estimate at the current time step. +The length of each of the $N$ steps is 1/$N$ the original time step. + +The attitude estimation is propagated according to the model $f$: + +\begin{equation} + f = + \begin{bmatrix} + \dot{\phi} = p + (q \sin{\phi} + r \cos{\phi}) \tan{\theta} \\ + \dot{\theta} = q \cos{\phi} + r \sin{\phi}) + \end{bmatrix} +\end{equation} + +This propagated estimate is then used in the calculation of the Jacobian $A$. + +\begin{equation} + A = + \begin{bmatrix} + (q \cos{\phi} - r \sin{\phi})\tan{\theta} & \frac{q\sin{\phi} + r\cos{phi}}{\cos^2{\theta}} \\ + 0 & -q \sin{\phi} - r \cos{\phi} \\ + \end{bmatrix} +\end{equation} + +This Jacobian is then used to find a second-order approximation of the matrix exponential, $A_d$. + +\begin{equation} + A_d = I + \frac{T_s}{N} A + \frac{1}{2} \frac{T_s^2}{N^2} A^2 +\end{equation} + +Where $T_s$ is the length of a time step. +$A_d$ is then used to propagate the actual covariance of the estimate. + +The process noise due to model uncertainty is defined in the matrix $Q$, but the process uncertainty due to the use of the rate gyro measurements $Q_g$, has not been adjusted for yet. +This is done with the use of the matrix $G$ which takes into account the Coriolis effects of the gyro measurements. +The measurement variance is transformed into process noise due to gyro measurements. +All of these contribute into finding the current covariance of our estimate, $P$. +This is done by the following equation: + +$$P_a = A_d P A_d^\top + (Q + G Q_g G^\top) \frac{T_s^2}{N^2}$$ + +With this propagated estimate and covariance we are now ready for a measurement update. + +### Measurement Update + +A measurement update provides a check on our propagated estimate and we take this new information and fuse it into our estimate. +The Kalman filter allows us to optimally adjust our estimate, our tuned process noises, and the noise characteristics of our sensor given the measurement. +These noise characteristics are captured in a diagonal matrix, $R_{sensor}$. +The entries are the variances and for the attitude step this is defined as: + +\begin{equation} + R_{accel} = + \begin{bmatrix} + \sigma_{accel, x}^2 & 0 & 0 \\ + 0 & \sigma_{accel, y}^2 & 0 \\ + 0 & 0 & \sigma_{accel, z}^2 \\ + \end{bmatrix} +\end{equation} + +Using our estimate and a model set of equations $h$, we predict the measurements the accelerometer will produce. +We will then compare the actual and predicted measurements and optimally adjust our estimate with the new information. +Since measurements come in much faster than the model propagates the measurement step is run every time the propagated estimate is calculated. +!!! note + The estimator asssumes that measurements come in faster than/as fast the estimation timer is calls for estimation updates. + If this is not the case performance can actually go down for the estimation if you increase the frequency of the estimation updates. +The set of equations, $h$, that predict the 3 measurements of the accelerometer, $y$, is given by: + +\begin{equation} + h = + \begin{bmatrix} + q V_a \sin{\theta} + g \sin{\theta} \\ + r V_a \cos{\theta} - p V_a \sin{\theta} - g \cos{\theta} \sin{\phi} \\ + -q V_a \cos{\theta} - g \cos{\theta} \cos{\phi} \\ + \end{bmatrix} +\end{equation} + +This yields a Jacobian $C$: + +\begin{equation} + C = + \begin{bmatrix} + 0 & q V_a \cos{\theta} + g \cos{\theta} \\ + -g \cos{\phi}\cos{\theta} & -r V_a \sin{\theta} - p V_a \cos{\theta} + g \sin{\phi} \sin{theta} \\ + g \sin{\phi}\cos{\theta} & (q V_a + g \cos{\phi}) \sin{\theta} + \end{bmatrix} +\end{equation} + +Which is used in finding the Kalman gain $L$. +An intermediate value is calculated called $S^{-1}$. +This value is: + +\begin{equation} + S^{-1} = (R_{accel} + CPC^\top)^{-1} +\end{equation} + +This intermediate value is then used to find $L$: + +\begin{equation} + L = PC^\top S^{-1} +\end{equation} + +The optimal estimate is then found: + +\begin{equation} + \hat{x}_a^+ = \hat{x}_a^- + L (y - h) +\end{equation} + +Finally, we update the covariance from our new estimate: + +\begin{equation} + P^+ = (I - LC) P^- (I - LC)^\top + LR_{accel}L^\top +\end{equation} + +We repeat this cycle until termination of the program. +This estimation scheme split into two parts allows for a clear set of equations and how the estimates affect one another. +We will now move on to the next portion of the estimator that estimates the rest of the states. + +## Position Estimation + +The position estimation step follows the same algorithm as previous. +Only new values are used for each of the matrices. +Those new entries are shown here, but reference the previous section for details on implementation. + +### Propagation + +The attitude estimation is propagated according to the model $f$: + +\begin{equation} + f = + \begin{bmatrix} + V_g \cos{\chi} \\ + V_g \sin{\chi} \\ + \dot{V_g} \\ + 0 \\ + 0 \\ + \dot{\psi} \\ + \end{bmatrix} +\end{equation} + +Where, + +\begin{equation} + \dot{\psi} = q \sin{\phi} + r \frac{\cos{\phi}}{\cos{\theta}} \\ + \dot{V_g} = \frac{V_a}{V_g} \dot{\psi} (w_e \cos{\psi} - w_n \sin{\psi}) +\end{equation} + +This propagated estimate is then used in the calculation of the Jacobian $A$. + +\begin{equation} + A = + \begin{bmatrix} + 0 & 0 & \cos{\chi} & -V_g \sin{\chi} & 0 & 0 & 0 \\ + 0 & 0 & \sin{\chi} & V_g \cos{\chi} & 0 & 0 & 0 \\ + 0 & 0 & -\frac{\dot{V_g}}{V_g} & 0 & -\dot{\psi} V_a \frac{\sin{\psi}}{V_g} & \dot{\psi} V_a \frac{\cos{\psi}}{V_g} & -\dot{\psi} V_a (w_n \cos{\psi} + w_e \sin{\psi}) \\ + 0 & 0 & -\frac{g}{V_g^2} \tan{\phi} & 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} +\end{equation} + +This Jacobian is then used to find a second-order approximation of the matrix exponential, $A_d$. + +\begin{equation} + A_d = I + \frac{T_s}{N} A + \frac{1}{2} \frac{T_s^2}{N^2} A^2 +\end{equation} + +Where $T_s$ is the length of a time step. +$A_d$ is then used to propagate the actual covariance of the estimate. + +The process noise due to model uncertainty is defined in the matrix $Q$ and is fused into the covariance. +This is done by the following equation: + +$$P_a = A_d P A_d^\top + Q \frac{T_s^2}{N^2}$$ + +With this propagated estimate and covariance we are now ready for a measurement update. + +### Measurement Update + + + +Because the GPS measures come in slower than the model propagates, the measurement step is only run when there is new GPS information. +This process is identical to the measurement update in the attitude step. +This will likely change before release, but the only difference is that it is done one measurement at a time. +This has advantages for querying the values while debugging, but is on the whole less clear and is more error prone. +The math is the same but it is carried through, and expressed as the final sum and each row calculated separately. + +The measurement model, $h$ has only 6 entries instead of 7 since heading, $\psi$ is not measured. +A digital compass could be added and $h$ and $C$ expanded if desired, but control is operated on course, $\chi$, so this proves largely unnecessary. + +\begin{equation} + h = + \begin{bmatrix} + p_n \\ + p_e \\ + V_g \\ + \chi \\ + V_a \cos{\psi} + w_n - V_g \cos{\chi} \\ + V_a \sin{\psi} + w_e - V_g \sin{\chi} \\ + \end{bmatrix} +\end{equation} + +This yields a Jacobian $C$: + +\begin{equation} + C = + \begin{bmatrix} + 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ + 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ + 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ + 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ + 0 & 0 & -\cos{\chi} & V_g\sin{\chi} & 1 & 0 & -V_a\sin{\psi} \\ + 0 & 0 & -\sin{\chi} & -V_g\cos{\chi} & 0 & 1 & V_a\cos{\psi} \\ + \end{bmatrix} +\end{equation} + +Which is used in finding the Kalman gain $L$. + +The measurement noise matrix, $R_{position}$, is defined as: + +\begin{equation} + R_{position} = + \begin{bmatrix} + \sigma_{gps, n}^2 & 0 & 0 & 0 & 0 & 0 & 0 \\ + 0 & \sigma_{gps, e}^2 & 0 & 0 & 0 & 0 & 0 \\ + 0 & 0 & \sigma_{gps, V_g}^2 & 0 & 0 & 0 & 0 \\ + 0 & 0 & 0 & \sigma_{gps, \chi}^2 & 0 & 0 & 0 \\ + 0 & 0 & 0 & 0 & \sigma_{pseudo, w_n}^2 & 0 & 0 \\ + 0 & 0 & 0 & 0 & 0 & \sigma_{pseudo, w_n}^2 & 0 \\ + 0 & 0 & 0 & 0 & 0 & 0 & \sigma_{pseudo, \psi}^2 \\ + \end{bmatrix} +\end{equation} + +An intermediate value is calculated called $S^{-1}$. +This value is: + +\begin{equation} + S^{-1} = (R_{position} + CPC^\top)^{-1} +\end{equation} + +This intermediate value is then used to find $L$: + +\begin{equation} + L = PC^\top S^{-1} +\end{equation} + +The optimal estimate is then found: + +\begin{equation} + \hat{x}_a^+ = \hat{x}_a^- + L (y - h) +\end{equation} + +Finally, we update the covariance from our new estimate: + +\begin{equation} + P^+ = (I - LC) P^- (I - LC)^\top + LR_{accel}L^\top +\end{equation} + +We repeat this cycle until termination of the program. + +## Software Architecture + +### Estimator implementation Specifics + +The estimator calls the update of the estimate on a timer. +Between estimation updates, the measurements come in as they are published and are saved in the estimator. +This allows asynchronous measurements and for only the most recent measurements to be used. +It does mean however, that the propagation is reliant on the measurements being faster or as fast as the estimation updates. +It also means that turning up the estimation frequency will not always have the anticipated effect. +To increase estimator performance a combination of process noise tuning, sensor uncertainty characterization, sensor update speed increase estimate update speed increase will be needed. + +### Replacing the Estimator + +To replace the estimator all that needs to be done is inherit from the `estimator_base` class and override the `estimate` method, as described in the .hpp file. + +| ![Estimator Override Function](../../../assets/estimator_assets/estimator_override_function.png "Estimator Override Function") | +|:--:| +|*Figure 1: The member method that must be overriden to use a new estimator.*| + +Next, replace `estimator_example.cpp` in the `CMakeLists.txt` with the newly created estimator file. + +| ![Estimator CMake Change Location](../../../assets/estimator_assets/estimator_cmake.png "Estimator CMake Change Location") | +|:--:| +|*Figure 2: The location to replace the file for the new estimator file.*| + +## Parameters + +| **Parameter** | **Explanation** | **Type** | **Range** | +| :---: | :---: | :---: | :---: | +| `sigma_n_gps` | The standard deviation of gps measurements in the north axis. | double | ~0.01 meters | +| `sigma_e_gps` | The standard deviation of gps measurements in the east axis. | double | ~0.01 meters | +| `sigma_Vg_gps` | The standard deviation of gps ground speed measurements. | double | ~0.005 $\frac{m}{s}$ | +| `sigma_course_gps` | The standard deviation of gps ground course measurements. | double | ~0.00025 radians | +| `sigma_accel` | The standard deviation of accelerometer measurements. | double | ~0.025 $\frac{m}{s^2}$ | +| `sigma_pseudo_wind_n` | The standard deviation of pseudo-measurement along the north axis. | double | ~0.01 | +| `sigma_pseudo_wind_e` | The standard deviation of pseudo-measurement along the east axis. | double | ~0.01 | +| `sigma_heading` | The standard deviation of heading measurement. | double | ~0.01 radians | +| `lpf_a` | The lowpass filter cutoff frequency for the gyro and accelerometers. | double | ~50.0 Hz | +| `lpf_a1` | The lowpass filter cutoff frequency for the pressure sensors. | double | ~8.0 Hz | +| `gps_n_lim` | The limit of GPS measurements in the north direction. | double | $\geq 10000$ meters | +| `gps_e_lim` | The limit of GPS measurements in the east direction. | double | $\geq 10000$ meters | +| `roll_process_noise` | The process noise on the roll propagation. | double | ~0.0001 | +| `pitch_process_noise` | The process noise on the pitch propagation. | double | ~0.0000001 | +| `gyro_process_noise` | The process noise on the gyro inversion. | double | ~0.13 | +| `pos_process_noise` | The process noise on the position propagation. | double | 0.1 | +| `attitude_initial_cov` | Initial covariance entry for attitude. | double | 5.0 | +| `pos_n_initial_cov` | Initial covariance entry for position north. | double | 0.03 | +| `pos_e_initial_cov` | Initial covariance entry for position east. | double | 0.03 | +| `vg_initial_cov` | Initial covariance entry for groundspeed. | double | 0.01 | +| `chi_initial_cov` | Initial covariance entry for course. | double | 5.0 Degrees | +| `wind_n_initial_cov` | Initial covariance entry for wind north. | double | 0.04 | +| `wind_e_initial_cov` | Initial covariance entry for wind east. | double | 0.04 | +| `psi_initial_cov` | Initial covariance entry for heading. | double | 5.0 | +| `num_propagation_steps` | Number of propagation steps to run between measurements. | int | 10 | +| `max_estimated_phi` | The maximum roll estimate before estimator saturation. | double | ~90.0 degrees | +| `max_estimated_theta` | The maximum pitch estimate before estimator saturation. | double | ~80.0 degrees | +| `estimator_max_buffer` | The buffer that the esimator returns the estimate to after saturation. | double | ~3.0 degrees | diff --git a/docs/developer-guide/rosplane/estimator/estimator-overview.md b/docs/developer-guide/rosplane/estimator/estimator-overview.md new file mode 100644 index 0000000..ce25e43 --- /dev/null +++ b/docs/developer-guide/rosplane/estimator/estimator-overview.md @@ -0,0 +1,95 @@ +# Estimator Overview + +## Overview + +The estimator is a continuous-discrete Kalman Filter, a full treatment of the filter is found in section 8.5 of the [UAV book](https://github.com/randybeard/mavsim_public). +This filter essentially works by inverting a few sensor models and then using a two stage estimation of first the attitude and then the position state values. +This page will outline the states, their meaning and any general notes on the states. +For a more in depth look at which states are estimated in which way visit the [Estimator Base](./estimator-base.md) and [Estimator Example](./estimator-example.md) pages. + +## ROS Interactions + +The estimator takes in sensor information from `rosflight_io` computes an estimate and publishes it to the rest of ROSplane. + + +| ![Diagram of Estimator ROS Interactions](../../../assets/estimator_assets/estimator_full_ros_interactions.png "Diagram of Estimator ROS Interactions") | +|:--:| +|*Figure 1: ROS network interactions for the estimator.*| + +### Input + +The inputs to the estimator are, accelerometer, rate gyro, barometer, differential pressure, GPS position, and GPS velocity estimates. +The table with the topic for each of the measures is below. + +| Measure | Explanation | Topic | +|:------:|:-------:| :---: | +| Accelerometer | This measures the specfic force applied to the aircraft in the body frame axes (see section 7.1 in the UAV book for more details). | `/imu/data/linear_acceleration` | +| Rate Gyro | This measures the angular velocity of the aircraft around the body frame axes. | `/imu/data/angular_velocity` | +| Barometer | The barometer measures the ambient air pressure. It is calibrated on arm to establish a "zero" altitude measurement. | `/baro/pressure` | +| Differential Pressure | The differential pressure sensor, measures the difference in pressure using a pitot tube due to forward velocity. | `/airspeed/differential_pressure` | +| GNSS Position | GNSS postion gives the position of the aircraft in latitude, longitude and altitude. | `/navsat_compat/fix` | +| GNSS Velocity | GNSS velocity gives the velocity of the aircraft in meters per second in the global NED frame. | `/navsat_compat/vel` | +| Status | Indicates whether the aircraft is armed (indicating a need to initialize position and altitude estimates). | `/status` | + +These topics provide the measures that are fused to create a state estimate. + +| ![Diagram of Estimator ROS IO](../../../assets/estimator_assets/estimator_ros_input_output.png "Diagram of Estimator ROS IO") | +|:--:| +|*Figure 1: ROS topic subscriptions and publications for the estimator.*| + +## Output + +There are 20 states estimated by the estimator that are published to the rest of ROSplane. +These states cover the position, orientation and aerodynamic information for the aircraft. +Below is a table of the `/estimated_state` message and what each of the fields represents. +Note that there are more than 20 states listed in the following table, but this is because either Euler angles or quaternions can be used to express orientation. +See the Frames and Derivation page for more information, or chapter 2 of the [UAV book](https://github.com/randybeard/mavsim_public). + +| State | Explanation | Range/Type/Units | +|:--:|:--:|:--:| +| postion[3] | A 3 vector of the NED position of the aircraft. | $(-\infty , \infty)$ (float)(meters) | +| va | The airspeed of the aircraft. Always positive because it is the magnitude. | $(0 , \infty)$ (float)(meters/second) | +| alpha | The angle of attack of the aircraft wing. | $(-\pi, \pi)$ (float)(radians) | +| beta | The side-slip angle of the aircraft. | $(-\pi, \pi)$ (float)(radians) | +| phi | The roll angle of the aircraft. | $(-\pi, \pi)$ (float)(radians) | +| theta | The pitch angle of the aircraft. | $(-\pi, \pi)$ (float)(radians) | +| psi | The yaw angle of the aircraft. | $(-\pi, \pi)$ (float)(radians) | +| chi | The course angle of the aircraft. | $(-\pi, \pi)$ (float)(radians) | +| u | The velocity in the body x axis. | $(-\infty, \infty)$ (float)(meters/second) | +| v | The velocity in the body y axis. | $(-\infty, \infty)$ (float)(meters/second) | +| w | The velocity in the body z axis. | $(-\infty, \infty)$ (float)(meters/second) | +| p | The angular velocity about the body x axis (rollrate). | $(-\infty, \infty)$ (float)(radians/second) | +| q | The angular velocity about the body y axis (pitchrate). | $(-\infty, \infty)$ (float)(radians/second) | +| r | The angular velocity about the body z axis (yawrate). | $(-\infty, \infty)$ (float)(radians/second) | +| vg | The groundspeed of the aircraft. Always positive because it is the magnitude. | $(0 , \infty)$ (float)(meters/second) | +| wn | The global north velocity of the wind. | $(-\infty , \infty)$ (float)(meters/second) | +| we | The global east velocity of the wind. | $(-\infty , \infty)$ (float)(meters/second) | +| _ | **The following entries are optional.** | _ | +| quat[4] | A 4 vector of the quaternion describing the orientation. | $(-1 , 1)$ (float) | +| quat_valid | A flag indicating whether the data in the quat entry is valid.| True/False (bool) | +| psi_deg | The yaw angle of the aircraft in degrees. | $(-180, 180)$ (float)(degrees) | +| chi_deg | The course angle of the aircraft in degrees. | $(-180, 180)$ (float)(degrees) | +| init_lat | The latitude of the aircraft when first armed. | $(-90, 90)$ (float)(DDS) | +| init_long | The longitude of the aircraft when first armed. | $(-180, 180)$ (float)(DDS) | +| init_alt | The altitude of the aircraft when first armed. | $(0, \infty)$ (float)(meters) | + +!!! note + More states may be estimated in the estimator than listed, these states are just those useful to other parts of ROSplane. + +## Running the Estimator + +The estimator is in the main `rosplane` ROS package. +The ROS executable is `rosplane_estimator_node`, yielding the run command: + +`ros2 run rosplane rosplane_estimator_node` + +To pass a set of parameters for the controller from a yaml file using the `--ros-args` option. + +`ros2 run rosplane rosplane_estimator_node --ros-args --params-file path/to/params.yaml` + + +!!! note + Filepaths will need to be altered to work. +!!! note + The most common way of running the estimator is through a launch file with the rest of the ROSplane pipeline running as well. + See the ROSplane Overview in the Developer and User Guides for more details. diff --git a/mkdocs.yml b/mkdocs.yml index 798ae44..484053b 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -77,6 +77,10 @@ nav: - Controller State Machine: developer-guide/rosplane/controller/controller-state-machine.md - Successive Loop Closure Controller Outline: developer-guide/rosplane/controller/controller-outline.md - Total Energy Controller: developer-guide/rosplane/controller/controller-total-energy.md + - Estimator: + - Estimator Overview: developer-guide/rosplane/estimator/estimator-overview.md + - Estimator Base: developer-guide/rosplane/estimator/estimator-base.md + - Estimator Example: developer-guide/rosplane/estimator/estimator-example.md - Parameter Management: developer-guide/rosplane/parameter-management.md - Path Planning: - Path Planning Overview: developer-guide/rosplane/path-planning/path-planning-overview.md