No issue but more a question?

[bvernham]

I am trying to understand the output of the kalman filter.

Essentially it looks like the kalman filter is providing the vertical acceration:

float verticalAccel = kalman.estimate(pastGyro, pastAccel, deltat);

But given the output of the Sentral, the linear acceleration can easily be calculated.

To quote the USFS wiki:

So if using EM7180 library from Simon Levy, and the above calculations, couldn't the Kalman filter be bypassed?

Thanks

Bruce

[juangallostra]

@bvernham if the linear acceleration obtained from the Sentral data is with respect to the inertial reference frame (which, from what I understand, seems to be the case) and not the body frame, then yes. The whole point of the implemented Kalman filter is to estimate the vertical acceleration in the inertial reference frame so that it can be used to estimate velocities and position.

If interested, Simon developed this sketch that uses the Sentral to obtain the data required by the filter. I haven't tested it so I can't tell if it works or how good the estimations are.

I, however, wasn't able to obtain results as good as the ones presented in the paper. I stopped trying to make this approach work long ago and instead moved to an error-state Kalman filter to estimate the desired states of a UAV.

[bvernham]

Based on the Wiki it would seem the inertial frame of reference can be calculated quite easily.

I have asked Simon Levy the same question.

Heading, Pitch, and Roll

In order to convert quaternion data into absolute orientation data reported as Euler angles (also known as heading, pitch, and roll), the following relations can be used:

Heading = atan2[(Qx2 – Qy2 – Qz2 + Qw2), 2(QxQy + QzQw)] Pitch = asin[-2(QxQz – QyQw)] Roll = atan2[(–Qx2 – Qy2 + Qz2 + Qw2), 2*(QxQw + QyQz)] ,

where the quaternions Qx, Qy, Qz, Qw are the outputs from the SENtral in the NED convention and the results are in radians. In order to convert to degrees multiply the result by 180/pi. (pi = 3.14159…).

The atan2 function produces values between –pi and +pi. In order to have the heading behave like a compass and report from 0 to 360 degrees, translate by (add) 2pi whenever the heading result is negative (<0) before converting to degrees.

Verify that the SENtral is configured properly by confirming that the heading varies between 0° – 360°. (i.e. it matches what you would expect on a compass) and increases in the clockwise direction. Also that the pitch increases when pitching upward and the range is ±180°, and that the roll increases when rolling clockwise and the range is ±90°.

Lastly, since magnetic North and True North can deviate depending on the magnetic declination, some consideration of this effect can produce a more accurate absolute orientation. For example, the magnetic declination was 13 degrees 40 minutes East in San Francisco on 9/18/2015, meaning one has to add 13.7 degrees to the heading to get True North. It is better to do this before the addition of 2pi in the above translation test to avoid large discontinuities in heading.

BTW, I am interested in you new work. It is out on GitHub?

Thanks

Bruce

[simondlevy]

Bruce, thanks for you contributions here. I did get your email yesterday and couldn't figure out what code you were referring to (kalman), but at some point I realized that you must have been looking at Juan's repository, as confirmed here.

So I think there are two separate issues here:

(1) Altitude estimation: As you have probably figured out, Juan has been working on the altitude estimation from IMU and barometer. It's also worth noting that he is interested in a more general approach that will work with any IMU and does not require the SENtral unit. I made a few attempts to do that myself, but basically gave up because of the difficulty of the calculations in involved. For altitude estimation, I am finding it easier in the short term to use just a high-quality distance sensor like the VL53L1X, as shown in this video, and not worry about fusing with IMU and barometer. Eventually I hope to incorporate Juan's ESKF work into Hackflight.

(2) Vehicle orientation: the Hackflight code already incorporates the quaternion-to-Euler conversion you describe above. I just updated the Hackflight README, and I think it's worth looking over to see our overall approach to these issues (simple, object-oriented, platform-independent code). So what I'm working on right now is using a Bluetooth connection to display the state (position, orientation, altitude) of the vehicle in a Python program that will run on your laptop as the vehicle is flying (basically, a simplified version of ROS rviz). This should give us a framework for investigating all these state-estimation issues efficiently.

[bvernham]

Thanks for the response.

to use just a high-quality distance sensor like the VL53L1X

My target is a ground vehicle not and air born vehicle so a distance sensor will not work.

I work with commercial so road grade is a very important factor to appropriately simulate the load applied to the engine. The GPS by itself is not good to pick up small rises and dips along with long continuous slight grades.

This is why I need to have the best possible estimation of altitude to be fused with the GPS data to get the best possible outcome.

After I get this part work I plan in implementing the following:[https://github.com/bolderflight/uNavINS(url)

I you have any thoughts they are appreciated.

Thanks

Bruce

[simondlevy]

Bruce, it seems to me that if you intend to use Bolderflight's uNavINS library, you should drop the EM7180 and go with the MPU9250 that Bolderflight is using in his example sketch. Kris sells a variety of them, like this one for Teensy, as do other vendors like Sparkfun. I've used Bolderflight's libraries before, and I can tell you that if he's posted something on github, it's solid and well-tested.

[bvernham]

Please elaborate on

if you intend to use Bolderflight's uNavINS library, you should drop the EM7180

I have looked at the code and most of it is doing exactly what the EM7180 is going as the update from of the IMU is much faster than the GPS (>10 times). So most of the time only the IMU states are updated.
if (newData == 1) { newData = 0; tstart = micros(); // read the sensor Imu.readSensor();

New data is interrupt driven from the MPU9250. Imu.enableDataReadyInterrupt();

All the code above the following is just doing what the Sentral is doing. '// Velocity Update dx = C_B2Nf_b + grav; vn_ins += _dtdx(0,0); ve_ins += _dtdx(1,0); vd_ins += _dtdx(2,0); // Position Update dxd = llarate(V_ins,lla_ins); lat_ins += _dtdxd(0,0); lon_ins += _dtdxd(1,0); alt_ins += _dt*dxd(2,0); ' When the new GPS reading comes in // Gps measurement update if ((TOW - previousTOW) > 0) { previousTOW = TOW; then the IMU and GPS data are fused.

So why not use the quaternion calculation from the Sentral?

If you look at the threads the are working on the AHRS (7 state kalman) and they are having drift problems which the Sentral calculation does not seem to have so I would believe the 15 state version as the same exact problems.

On other issue is the uNavINS library does not fuse the baro sensor which I have explained above GPS alone will not give a good enough grade calcification.

Any thoughts you may have on this are appreciated.

Thanks

Bruce

[juangallostra]

@bvernham another thing you could do is implement a new correction/measurement update step in the EKF a part from the GPS update step that fuses barometric data and helps to correct the estimated altitude. However, I am not sure a barometer will help in obtaining the accuracy and precision you want. My tests so far have not been that successful.

Anyway, when running an EKF for state estimation (which I see is the case of Bolderflight's uNavINS library) you can have different measurement models for different sensors and correct/update the estimated state with the incoming measurements whenever new data is available.

Here is a nice Mathworks example that shows what I mean (although there is fair amount of Simulink too).

The process would then be (assuming the IMU has the highest refresh rate): For every incoming IMU measurement,

1. Predict state from last update to the current
2. Check if new GPS data is available. If it is construct GPS measurement jacobian and update state.
3. Check if new barometer data is available. If it is construct barometer measurement jacobian and update state.

In the case both GPS and barometer data are available in the same iteration there is no problem in chaining the two measurement updates.

So, a part from the already implemented GPS measurement update step (https://github.com/bolderflight/uNavINS/blob/69cc7e3a0e2459c8c1121c06895d8de15c18afc6/uNavINS.cpp#L179) you would need a new barometer measurement update step that gets called whenever new barometric data is available.

[simondlevy]

Bruce: All I meant was that if you intended to use Bolderflight's uNavINS library as-is, you'd be better of working with the MPU9250, since that's the device he uses in his examples. Now that you describe the uNavINS in more detail, and mention the comparisons you've done between the MPU9250 and EM7180, I have to agree, your choice of the EM7180 makes more sense.

I wish I had more insights to offer, but at this point I think you understand the issues better than I do.

[juangallostra]

Closing due to inactivity.