How to add additional sensors to the mekf?

[memphis242]

Hey, so our project is trying to build an autonomous quadcopter that navigates indoors without the aid of GPS, and because of size and weight limitations, we can't really use a LiDAR and perform SLAM with that. That leaves just a few possibilities. Now I've set up the roscopter package and I notice that for the mekf node, it uses IMU, GPS, magnetometer, barometer, and sonar data from what is published by rosflight_io to estimate the current state. Without the GPS input however, as far as I understand, just using the IMU data will not be enough because eventually the error builds even if the initial error is very small. No one in my project is really familiar with how EKFs work, and aside from reading and researching on our own, I wanted to ask you guys how you would go about adding sensors to the mekf.

So for instance, one of the sensors that could help us is an optical flow sensor, and specifically, we have the PX4flow board and have it setup to work with the px4flow_node package, and that would give us x, y, x-velocity, and y-velocity values.

[superjax]

Hey @memphis242, cool project! The main motivation for our stuff is actually GPS-denied, however we haven't posted a lot of our GPS-denied code.

So, in general, to add sensors to an EKF you have to derive a "measurement model" and the associated jacobian.

For example, if you can write the estimated measurement from the PX4Flow in terms of the state where x is the state and y is the measurement.

y = h(x)

Then the Jacobian you need to implement is dh/dx (and should be a short and fat matrix).

the baroCallback, sonarCallback, gpsCallback and magCallback all create an h vector (the measurement) and an H matrix, (the jacobian). Once you have those matrices, you can plug them into the Kalman Filter update step, which is also in each of these callback functions.

Honestly, Kalman filtering can be pretty complicated. I would recommend at the very least reading Dr. Beard's notes on the subject (https://scholarsarchive.byu.edu/cgi/viewcontent.cgi?article=2324&context=facpub) (chapter 6)

I haven't looked at the filter in this package in a long while... it's kinda old, and we are in the middle of adapting our VI-EKF package that @jerelbn and I have been working on to be a bit more generic. That filter will ultimately replace the mekf in roscopter. Adding measurement models is a little easier in that package, and it's being actively maintained.

[superjax]

Also, now that I am thinking about it, if you can carry a camera and have the computing power onboard, I would reommend VINS. It's a really superb visual-inertial estimation package that works with almost anything.

[memphis242]

Ok, so I'll need some more help on this. After going through the code a bit more, as well as the paper (which I honestly was not really able to understand past the first chapter), here is what I'm getting. VI-EKF and VINS aside, say we want to add the optical flow data to the MEKF, this is what I have understood that I need to do (verify and correct me please):

• I need to add to the multirotor.yaml file variables related to the PX4Flow sensor. Now the sensors outputs flow_x, flow_y, and altitude, which are (delta)(pixels) in x, (delta)(pixels) in y, height in meters respectively. You can see this from this video here. Following from the GPS stuff in this file as it is the most similar to the optical flow data, I will need need to add a Rpx4flow variable for the covariance, put some flow_x stddev, flow_y stddev, alt stddev, k_flow_x, k_flow_y, k_alt, initial_flow_x, initial_flow_y, initial_alt, optflow_bias. Now, how on Earth do I get any of these values (aside from the initial stuff).

• Once I have those parameters, set, do I need to change P0, Qx, Qu, etc. or any of the functions in mekf.cpp that involve updating the covariance and so on?

• Once those parameters are set, I see that the callback function might look something like this?

  void kalmanFilter::optflowCallback(const px4flow_msgs::OpticalFlow msg)
  {
   int_16 flow_x = msg.flow_x;
   int_16 flow_y = msg.flow_y;
   uint_8 quality = msg.quality;
  
   //Convert to something like px4_nn, px4_ne, px4_nd, and then create Eigen::Vector3d 
   //px4_val(px4_nn, px4_ne, px4_nd)
  
   //measurement model
   Eigen::Vector3d h_px4f(p_(0), p_(1), p_(2));
  
   //measurement Jacobian
   Eigen::Matrix<double, 3, NUM_ERROR_STATES> H;
   H.setZero();
   H(0, dPN) = 1;
   H(0, dPE) = 1;
   H(0, dPD) = -1;
  
   //Compute residual covariance
   Eigen::Vector3d S = H * P_ * H.transpose() + R_px4flow;
  
   //Compute Kalman gain
   Eigen::Matrix<double, NUM_ERROR_STATES> K = P * H.transpose() * S.inverse();
  
   //Compute measurement error
   Eigen::Vector2d residual = px4f_val - h_px4f;
  
   //Compute delta_x
   Eigen::Matrix<double, NUM_ERROR_STATES, 1> delta_x = K * residual;
  
   //Update state & covariance
   stateUpdate(delta_x);
   P_ = ( (Eigen::MatrixXd::Identitiy(NUM_ERROR_STATES, NUM_ERROR_STATES) - K * H) * P_;
  
   return;
  }

• Anything else you could tell me would be super freaken helpful. We are very new and lack a lot of knowledge and this is overwhelming but we are also stubborn, so please.