EKF - Mag support

[superjax]

We need to support the magnetometer before we can get good position and velocity estimates.

@nonrevlb and @dpkoch, what would be left to do before we could use the magnetometer to resolve our heading? I can make sure that the latest ROSflight supports mag early this week. I have delayed getting the mag going because I wanted to merge it in at the same time I merged in the asynchronous stuff. However, I have been running into some weird issues getting multiple sensors reading asynchronously on ROSflight which has caused some delays. What if we just do asynchronous IMU, while blocking other sensors for now, until I can resolve those issues?

I remember that @dpkoch was working on getting mag calibrated and streaming from byu-magicc/fcu_io before I left, but I don't know what the state of that is currently. @nonrevlb, do you think that you could get it into the ekf? Unfortunately, I'm a little hazy on the measurement model. I'm pretty sure that it is in the UAV book, and if it's not, it is definitely somewhere online. We might need to do a little reading before we fully understand the measurement models, but once we have them, it should be easy to simulate and add to the EKF. What do you think?

I think this is the final step for the LANL project, I guess this and #10.

[nonrevlb]

There is a model for magnetometers in the UAV book. I understand most of it so I can get started putting it into the EKF.

I do have one question: The model applies bias error and sensor noise. I assume these are related to the alpha and beta terms in the state, but I don't really understand how they relate. How should the bias error and sensor noise be represented in the ekf?

[nonrevlb]

Also, what kind of message will the magnetometer make?

[superjax]

Great! Thanks @nonrevlb. The alpha and beta terms in the EKF are biases on the accelerometer and gyro measurements, respectively. We would need an additional 3 states as biases for the magnetometer, if we decided to estimate them. For now, let's not estimate magnetometer bias.

However, you're right, we do get some information, especially about the gyros biases in the magnetometer update. Do a similar thing as we did for the GPS. First, write the vector of measurements as a function of the states and inputs. The magnetometer measurement will include the inputs, which are az, p, q, and r. Then, take each of these input terms, and substitute the measurement, plus the bias, because

a_z = accel_z + alpha_z
p = gyro_x + beta_x
q = gyro_y + beta_y
r = gyro_z + beta_z

The alpha_z term shows up whenever you include az, and the beta terms show up when you use p, q, and r. I don't think that the GPS was a function of the inputs, so this will be a little more complicated, but not terrible. You'll end up with a few 1's in the Jacobian, on the bias terms.

Right this minute, I can't remember if we are assuming that the bias is positive, or negative. I might have derived it backwards, but I can't remember. You've gone through my accelerometer model, it should be a very similar model for the magnetometer. If you want, you can write it up first in the latex document, and I'll go through it.

[superjax]

The magnetometer will be a sensor_msgs/MagneticField

[nonrevlb]

I don't understand what are az, p, q, and r? And where do these come into the magnetometer measurements? As far as I can tell, the magnetometer measurements are only related to phi, theta, ans psi. In the IMU part of the pdf, it just drops all the terms with p, q, and r because the coriolis terms are small.

[superjax]

Oh, yeah. You're right. I think it will be independent of the bias terms then.

[nonrevlb]

I wrote everything up for the magnetometer on the latex document. Could you take a look at it?

[superjax]

I went through the latex doc. I don't have the UAV book, so I didn't check eq. 17. Assuming that 17 is right, then all the terms in 19 are right. Nice work!

I do have one recommendation. Before merging, let's change the known magnetic field vector to something other than a. It took me a second to figure out what you were talking about because I am used to a being used for acceleration vectors. Let's use something else, what about b?

[nonrevlb]

The UAV book didn't seem to have exactly what I needed to make an EKF. It calculated the heading based on the measured magnetic field but there wasn't an easy way to isolate the components of the magnetic field in order to take partial derivatives. But based on how it described the magnetometer working, I figured that the measured magnetic field is simply the global magnetic field transformed into the robot frame. Does that sound right? That is where eq 17 comes from. I added to the latex document to make it clear how I calculated eq 17.

In order to make sense of the measured magnetic field, it has to know what the magnetic field is at it's current location. I was thinking of using the code from http://www.ngdc.noaa.gov/geomag/WMM/soft.shtml to do this. Is there a better way?

[superjax]

Awesome! I think that is exactly the right approach.

Yeah, it'd be sweet to use that data. Is the code overly complicated? Mag is returned 160 times per second, so it may make sense to run it only on initialization, and assume that it is constant throughout the flight.

I'll go through the derivation of eq. 17 later this evening, but It looks right. I spend some time thinking about the angles, and it made sense. What's the approach now? Do we need to set up a simulation?

As for my tasks, I just made a pull request for the async branch yesterday, which has magnetometer (and a super-snazzy circular buffer for asynchronously reading i2c). I'm sure you're more aware of the state of mag calibration than I am.

[nonrevlb]

The code from WMM seems easy enough to use. I'm just having trouble accessing it's functions from within ekf.cpp. I've got the header from WMM included and it recognizes all of the structs declared there but not the functions.

The next step is probably setting up a simulator so we can test it. Do you want to do that our should I try?

[superjax]

I'll go ahead and set up the simulator. You get the ekf going, and I think you can work with @dpkoch on getting the calibration going.

[nonrevlb]

I can't get the WMM code to work. It calls some memory allocation functions that use malloc that throw errors. The only way I've been able to convince ros to find the file is to change it from .c to .cpp. But a physics professor I was talking to told me that malloc is incompatible with c++. Any thoughts on how to fix this? Or ideas on other ways to determine what the magnetic field should be in a given location?

Meanwhile, I've been testing things with the same magnetic field the simulation uses. I turned off the IMU and GPS so only the magnetometer is updating. When I do that, phi, theta, and psi all are nearly 0 and don't really change. I figure that maybe it isn't trusting my magnetometer measurements at all, but I thought changing the R matrix would fix this but it doesn't seem to make a difference. What else could make it do this?

[nonrevlb]

I'm having a problem with fcu_sim. About 3/4 of the time when I run it, it crashes and prints this:

X Error of failed request:  BadDrawable (invalid Pixmap or Window parameter)
  Major opcode of failed request:  155 (DRI2)
  Minor opcode of failed request:  3 (DRI2CreateDrawable)
  Resource id in failed request:  0x4a00002
  Serial number of failed request:  37
  Current serial number in output stream:  39

[gazebo_gui-3] process has died [pid 2498, exit code 1, cmd /opt/ros/indigo/lib/gazebo_ros/gzclient __name:=gazebo_gui __log:=/home/magicc/.ros/log/57145116-a1fd-11e6-9a7e-2c27d71e3512/gazebo_gui-3.log].

Any ideas?

[nonrevlb]

I was able to tune everything to get the magnetometer working pretty well. This is what it looks like with an IMU and Magnetometer:

[nonrevlb]

With magnetometer noise on, I get this: Is this accurate enough? Is there anything I can do to improve it?

[superjax]

@nonrevlb strikes again!

This is awesome, and those are fantastic results. You can only track so well with noise, and we don't want to spend too much time tuning it in simulation, since we'll likely have to do it again in hardware.

Put up a merge request!

[superjax]

the X driver spoken of in your error is the window manager of linux. This is going to sound stupid, but have you restarted the computer? That's a weird bug that is likely unique to your computer. I certainly haven't seen it.

[nonrevlb]

I was using the y component of the quaternion for testing and it was working great. Before merging, I checked the x and z components and they aren't working. So I need to figure that out before I create a merge request.

The ground truth I'm reading for z doesn't make sense to me. The graph looks like this: Why would the z component of the quaternion be 0? I've got it moving between waypoints (0,5,-2), (5,0,-2), (0,-5,-2), and (-5,0,-2) so it is tilting in lots of different ways. I would expect z to change for some orientation. Is this right? Where is shredder/ground_truth being published?

[superjax]

That is weird. You're right, it should be changing.

[jerelbn]

removed ekf, now using mekf