Adding-Sensor-Models.md

Contents

• Contents
• Introduction
• Step-by-Step
  • Duplicate Pacejka Sensors
  • GPS Sensor
  • IMU Sensor
  • Admin Files
  • Review
• Up Next

Introduction

Now that we have defined our system dynamics model we can turn our attention to the sensor models, located in src/crs/sensor_models/. Again, we will use the already defined Pacejka sensor model as a template to save time and will use of the 4 DoF parafoil system as an example.

In general the sensor model $h(x,u)$ is some nonlinear function of the states $x$ and inputs $u$ and these dynamics can be implemented similarly to the system model we already wrote. However, we also notice that we oftentimes are measuring rates of change (accelerations, rotation rates, etc.), and our system dynamics model already defines the rate of change of our states (accelerations, rotation rates, etc.). We can leverage this to make our lives easier! By calling our system model we can get the state derivative $\dot{x}=f(x,u)$ for our current state and input and then apply a simple transform $y=\tilde{h}(\dot{x})$ to model the correct sensor measurements. So in effect our sensor model is then:

$$\begin{align} y = h(x,u) = \left(\tilde{h} \circ f_{\mathrm{model}}\right)(x,u) \end{align}$$

The parafoil system we are using as an example has 2 main sensors: a GPS and an IMU. The GPS measurements are simply the inertial position and velocity, both of which can be calculated directly from the current state $x$ (not the state derivative $\dot{x}$), so this sensor model will not require the dynamics model. However, the IMU model will call the dynamics model to get the state derivative $\dot{x}$ which will be transformed to the appropriate body frame rotation rates $p,q,r$ and accelerations $a_1, a_2, a_3$.

Step-by-Step

Duplicate Pacejka Sensors

Duplicate the pacejka_sensor_model/ directory and rename, e.g. to parafoil_4dof_sensor_model/

pacejka_sensor_model
├── CMakeLists.txt
├── include
│   └── pacejka_sensor_model
│       ├── imu_sensor_model.h
│       └── mocap_sensor_model.h
├── package.xml
└── src
    ├── imu_sensor_model.cpp
    └── mocap_sensor_model.cpp

GPS Sensor

Update header files

1. Rename file from mocap_sensor_model.h e.g. to gps_sensor_model.h
2. Update ifdefs
3. Find & replace all pacejka_ with parafoil_4dof_
4. Remove all car_ (missed by find & replace)
5. Rename class from MocapSensorModel e.g. to GPSSensorModel
6. Update measurement output size

Update source files

1. Rename file from mocap_sensor_model.cpp e.g. to gps_sensor_model.cpp

2. find & replacce all pacejka_ with parafoil_4dof_

3. remove all car_ (missed by find & replace)

4. find & replace all mocap with GPS

5. find & replace all MocapSensorModel with new class name e.g. GPSSensorModel

6. Update constructor which defines casadi function containing dynamics. Shown below is the example from the parafoil gps sensor

   /**
   * @brief Construct a new GPS Sensor Model. 
   *
   * @param R measurement covariance Matrix
   */
   GPSSensorModel::GPSSensorModel(const Eigen::Matrix<double, 6, 6>& R)
   : SensorModel(6, GPSSensorModel::SENSOR_KEY)  // Measurement dimension is six
   {
   std::vector<casadi::MX> state_mx = { casadi::MX::sym("x"),   casadi::MX::sym("y"),   casadi::MX::sym("z"),
                                       casadi::MX::sym("phi"), casadi::MX::sym("theta"), casadi::MX::sym("psi"),
                                       casadi::MX::sym("u"), casadi::MX::sym("v"), casadi::MX::sym("w") };
   // x, y, z, v_x, v_y, v_z
   std::vector<casadi::MX> measured_states_mx = { state_mx[0], state_mx[1], state_mx[2],
                                                   cos(state_mx[3])*1*state_mx[6] + (cos(state_mx[5])*0*sin(state_mx[3]) - sin(state_mx[5])*cos(state_mx[3]))*state_mx[7] + (cos(state_mx[5])*0*cos(state_mx[3]) + sin(state_mx[5])*sin(state_mx[3]))*state_mx[8],
                                                   sin(state_mx[5])*1*state_mx[6] + (sin(state_mx[5])*0*sin(state_mx[3]) + cos(state_mx[5])*cos(state_mx[3]))*state_mx[7] + (sin(state_mx[5])*0*cos(state_mx[3]) - cos(state_mx[5])*sin(state_mx[3]))*state_mx[8],
                                                   -0*state_mx[6] + 1*sin(state_mx[3])*state_mx[7] + 1*cos(state_mx[3])*state_mx[8] };
   measurement_function = casadi::Function("applyMeasurementModel", state_mx, measured_states_mx);
   R_ = R;
   }

7. Update all matrix sizes to match sensor output and state dimensions

IMU Sensor

Update header files

1. File already named imu_sensor_model.h
2. Update ifdefs
3. Find & replace all pacejka_ with parafoil_4dof_
4. remove all car_ (missed by find & replace)
5. Class is already named ImuSensorModel
6. Update measurement output size (if necessary)

Update source files

1. File already named imu_sensor_model.cpp

2. Find & replace all pacejka_ with parafoil_4dof_

3. Remove all car_ (missed by find & replace)

4. Class is already named ImuSensorModel

5. Ensure correct dynamics model is loaded i.e. ContinuousParafoil4dofModel instead of ContinuousPacejkaModel

6. Update constructor which defines casadi function containing dynamics. Shown below is the example from the parafoil gps sensor.

   /**
   * @brief Construct a new IMU Sensor Model. Note that the accelerations are not part of the 4 DoF parafoil state and 
   * therefore the continuous model is needed
   *
   * @param parafoil_4dof_cont the continuous model
   * @param R measurement covariance Matrix
   */
   ImuSensorModel::ImuSensorModel(const std::shared_ptr<crs_models::parafoil_4dof_model::ContinuousParafoil4dofModel> parafoil_4dof_cont,
                               const Eigen::Matrix<double, 6, 6>& R)
   : SensorModel(6, ImuSensorModel::SENSOR_KEY)  // Measurement dimension is six
   {
   std::vector<casadi::MX> state_mx = { casadi::MX::sym("x"),   casadi::MX::sym("y"),   casadi::MX::sym("z"),
                                       casadi::MX::sym("phi"), casadi::MX::sym("theta"), casadi::MX::sym("psi"),
                                       casadi::MX::sym("u"), casadi::MX::sym("v"), casadi::MX::sym("w") };
   std::vector<casadi::MX> input_mx = { casadi::MX::sym("delta_s"), casadi::MX::sym("delta_a") };
   
   auto cont_dynamics = parafoil_4dof_cont->getContinuousDynamics(state_mx, input_mx); 
   //                                             p,                q,                                r,
   //                                             accel_1,          accel_2,                          accel_3,
   std::vector<casadi::MX> measured_states_mx = { cont_dynamics[3], sin(state_mx[3])*cont_dynamics[5], cos(state_mx[3])*cont_dynamics[5],
                                                   cont_dynamics[6], cont_dynamics[7] + 9.81*sin(state_mx[3]), cont_dynamics[8] + 9.81*cos(state_mx[3]) };
   
   state_mx.insert(state_mx.end(), input_mx.begin(), input_mx.end());  // Append input at the end of state vector
   measurement_function = casadi::Function("applyMeasurementModel", state_mx, measured_states_mx);
   R_ = R;
   }

   Note that the constructor instantiates a continuous model which is called to generate the state derivatives.

7. Update all matrix sizes to match sensor output and state dimensions

Admin Files

Update CmakeLists.txt

1. Update project name
2. Update add_library block to match names of sensor files

Update package.xml

1. Update package name
2. Update maintainer name & email (if necessary)
3. Update dependencies to include dynamics model

Review

Folder structure should then be equivalent to the following:

parafoil_4dof_sensor_model
├── CMakeLists.txt
├── include
│   └── parafoil_4dof_sensor_model
│       ├── gps_sensor_model.h
│       └── imu_sensor_model.h
├── package.xml
└── src
    ├── gps_sensor_model.cpp
    └── imu_sensor_model.cpp

Up Next

Now that we have both our system dynamics model and our sensor models we are ready to start building our system architecture in ROS. First we should define our message files before we forget.