Southern Hemisphere

[danni]

Hi,

Just trying to understand what, if any, issues affect alignment in the Southern Hemisphere. I'm just getting started with INS and what I'm seeing is data that bends backwards with respect to the GPS. It feels a lot like one of the sensors vectors is upside down, but the alignment appears not to compensate for it. I couldn't find much in the literature about INS in the southern hemisphere except a reversed angular velocity, but I failed to follow how that worked in the maths.

(The orange line is GPS samples, the purple line is IMU samples and the blue line is the filtered combination).

[nmayorov]

Hi! From the first look the filtered result looks reasonable. Why do you believe that something is wrong with the alignment procedure? What was the computation procedure to get this plot?

[danni]

I don't specifically think something is wrong and if you tell me it definitely works I'll investigate other options. I just found it curious how the track bent backwards symmetrically.

This plot is just lon/lat. The colour gradient was just time, to know everything was behaving correctly (I had previously had some problems ascertaining what units my sensors were in).

I've done a bit more experimenting and I've found that by changing the alignment window I can get very different results. So I actually reckon that your code is perfect and that there's something screwy in the data I'm using for alignment.

[danni]

For reference here's the implementation:

from pyins.integrate import coning_sculling, integrate
from pyins.align import align_wahba

dt = 0.01

accel = simudf.as_matrix(columns=['Accelerometer X', 'Accelerometer Y', 'Accelerometer Z']) / 1000 * dt * 9.81
gyro = simudf.as_matrix(columns=['Magnetometer X', 'Magnetometer Y', 'Magnetometer Z']) / 1000 * dt
theta, dv = coning_sculling(gyro, accel)

window = slice(100, 700)
theta_align = theta[window]
dv_align = dv[window]
(h0, p0, r0), _ = align_wahba(dt, theta_align, dv_align, sdf['Latitude'][0])

idf = integrate(dt, sdf['Latitude'][0], sdf['Longitude'][0], 0, 0, h0, p0, r0, theta, dv)

from pyins.filt import FeedforwardFilter, LatLonObs, InertialSensor

gyro_bias_sd = 0.05 / 3600  # 0.05 d/h
accel_bias_sd = 5e-3

gyro_noise = 1e-6  # deg / s^0.5
accel_noise = 3e-4  # m / s^1.5

gyro_model = InertialSensor(bias=gyro_bias_sd, noise=gyro_noise)
accel_model = InertialSensor(bias=accel_bias_sd, noise=accel_noise)

filt = FeedforwardFilter(dt, idf, pos_sd=10, vel_sd=0.1, azimuth_sd=0.5, level_sd=0.05,
                         gyro_model=gyro_model, accel_model=accel_model)
gps = pandas.DataFrame({'lat': sdf['Latitude'], 'lon': sdf['Longitude'], 'stamp': range(0, 900, 10)})
gps = gps.set_index('stamp')
gps = LatLonObs(gps, 5)

combined_df = filt.run(observations=[gps])

plt.scatter(sdf['Longitude'], sdf['Latitude'], c=range(len(sdf)), cmap='OrRd', label="GPS Track")
plt.scatter(idf['lon'], idf['lat'], c=range(len(idf)), cmap='PuRd', label="IMU Track")
plt.scatter(combined_df.traj['lon'], combined_df.traj['lat'], label="GPS + IMU Track")

[nmayorov]

Hey Daniel, looks interesting! You seem to get a good grasp on how to use it.

Would you be interested in somehow working on this repository? I sort of abandoned it, however now I'm again involved in navigation and sensor fusion problems and I want to improve and promote it. I think the code and idea are not that bad, but it needs farther development, better documentation and some promotion.

There are several points I can think of "right off the bat":

1. Better support for rate sensors, i.e. which measure instantaneous angular rate and acceleration (typical for modern MEMS). You seem to use it, however coning and sculling algorithms need to be adjusted to combine both numerical integration and non-commutative corrections. There is a decent paper on this subject.
2. Expand modeling capabilities. At least we should add proper IMU error models (not to be confused for sensor models used for Kalman filtering). I think we definitely need to add "semi-proper" GNSS modeling, i.e. simplified modeling of constellations with some simple error models for range and range-rates. Also some higher level trajectory generators would be very useful.
3. Some basic useful functions are missing: like ECEF to lat, lon, height conversion.
4. More navigation and alignment algorithms: at least some classical quasi-static alignment based on Kalman filtering should be implemented. "Wide-heading" alignment and navigation algorithms are also of interest.
5. Better documentation and code organization. This is a broad point, but we are free to change anything at this point.

I have some code addressing this points, but didn't prepare it properly.

So let me know what you think, you can contact me via e-mail (see in the commit log) or write here. Ask any questions you might have as well. And if you know someone who would be interesting --- bring them in.

[danni]

I'm not sure how much help I can be in the near term. I'm still getting my head around even the simple stuff. I only learned the phrase "strapdown INS" last week :-) I'm still in the proof of concept phase for integrating our IMU to stabilise our GPS. But it has to happen, so you're certain to be hearing from me.

One other thing, I also have quaternions. Is that a more reliable way to calculate initial heading, pitch and roll? I see there's a function to build a DCM from quaternions, but what's the order of parameters?

[nmayorov]

OK, will be happy to hear from you. Maybe I will suggest something for you to work on.

One other thing, I also have quaternions. Is that a more reliable way to calculate initial heading, pitch and roll? I see there's a function to build a DCM from quaternions, but what's the order of parameters?

Quaternions and DCMs are two equivalently good and accurate approaches, you can't get conceptually better results by using one of these.

[nmayorov]

Forgot to add that the order of elements in dcm.from/to_quat--- the vector part goes first, the 4-th elements is the scalar part.