It turns out that I had forgotten to convert from degrees to radians, and that made a significant difference. My angular data is maintaining precision at a level that I would expect. My positional data, on the other hand, quickly becomes unusable due to the slight angular error that causes the gravity vector to become misaligned and no longer accounted for. A little constant acceleration quickly becomes a large velocity and a very wrong position. This is where a Kalman filter would be helpful, perhaps. I could also probably look into measuring the total acceleration magnitude, and when it is about equal to Earth's gravity, assume the device is sitting still and recalibrate, which might help some, particularly when the device isn't moving much, like sitting on a table.
Tonight, I backported some improvements from my Kalman branch to master, worked on the general state of documentation, and wrote some conclusions. The conclusions are not final yet -- I am still actively working to make my angular state information more closely reflect reality, and if I do, that conclusion could easily change in its entirety.
After a lot of investigation, I found that the problems were indeed the fault of my code. Really, it was a result of too-similarly named types.
Triple is a struct of
Triple_F is a struct of
floats. In the
update_Qstate function I was using a
Triple when I meant to use a
Triple_F, which explains some of the problems I had been having, but not all of them. I still don't know how the argument was being corrupted, but I can confirm that the g++ on my x86 Chromebook is definitely generating a different binary than the g++ running on my Raspberry Pi 2. Admittedly, they are different versions, so that's more or less to be expected, but I haven't had any issues with argument corruption since switching to my Chromebook.
Interestingly enough, the compiler never warned me about storing
float values into
shorts, even though I have
-Wall on, until I started doing it in the initializer for the struct.. an interesting effect of automatic type coercion. I look forward to using Rust more in the future, where mistakes like this one cannot happen.
As part of this week's documentation, I've been characterizing the behavior of different pieces of my code so I can explain how well each part works, which means collecting tens of thousands of data points from various places in the code, running comparable computations in Python on my computer and comparing the answers, along with plotting how each variable is changing over time in order to see the bigger picture. Even though I double checked my quaternion math against the references I've found, I'm not entirely happy with the results I'm getting there. I believe the issue is that I could be using the wrong units (degrees vs radians), but I just decided that is likely as I was writing this post, so I haven't tested it.
Tonight, I started digging deeply into the data coming from my device, trying to get a feel for it, and I realized there was something rather interesting going on. Upon deeper inspection, I found that I had not properly been converting my gyroscopic values to degrees per second, and from there to a raw number of degrees.
Once I fixed that, I encountered a much stranger, and much more troubling issue: the gyro argument being passed in was corrupted, completely. I feared that the communications link to the LSM9DS0 might have been broken, but instrumenting the code one level up in the stack, before this function was called showed that the data was completely valid before being passed into the function, and then garbage once inside the function. It seems that passing a basic integer into the function works, but no attempt has been successful to make a Triple instance retain valuable data, whether it is an argument, a local variable, or a local pointer to a
new Triple. It refuses to retain data -- all of the data coming over the serial port is garbage, if it is stored in Triple struct. If I place each value of the triple into a separate, local float variable, the data is retained correctly. The gyro values seem perfectly reasonable, but right now my device claims to be in an uncontrolled spin, according a graph of the Quaternion state data vs time. I believe the
ao_quaternion structs are also being corrupted in this one function. If I can get to the root of this issue, I think the output data will be quite valuable, even without the Kalman filter, but I do plan to get the Kalman filter finished as well.
I was able to successfully integrate the CMSIS DSP library into my project, in the form of the
mbed-dsp package. My first attempt at this was not successful, due to some makefile issues, but taking an alternative approach was able to clear things up in (relatively) short order.
After getting that done, I now had the matrix functions I needed to properly manipulate my arrays. I have now written an initial implementation of the Kalman
predict function, making heavy use of the CMSIS DSP library functions. The code is untested as of this moment, and I am concerned that upgrading my
mbed library may cause unexpected, breaking changes in other parts of my code, just due to the nature of updating libraries attached to a project. I plan to make testing my current code (the new stuff and the new mbed version) part of my work tomorrow, but I expect to get other stuff done as well, hopefully. If so, there's every chance that by Monday I could have a functional (even if not properly tweaked) Kalman filter active in my code base! I find that prospect exciting.
GitHub link (kalman.h and kalman.cpp are where the current adventure is in progress)
I continued working on and looking into the Kalman filters tonight. Also of interest, apparently the CMSIS provides some basic matrix math functions, which was a pleasant surprise, and I plan to use them, if I can get them working.
Since the edits I'm doing are sweeping enough to make the code non-functional while the Kalman filter is being developed, I have created a separate branch in git to hold the kalman-enabled code. I have reasonable-seeming values populating most of the initial Kalman state, except for Q which is still to be determined. The next major step will be to implement the Kalman equations in the predict and update functions. Since I believe the covariance between the three axes would be zero, I plan to run three separate multivariate Kalman filters. Each filter has two hidden variables exposed in it, namely position and velocity. I also plan to implement Kalman filtering on the angular data as well, but I've started with the acceleration data.
This evening, I started making headway on the actual Kalman filter code.
Today's step on the path to Kalman filtering my data took me through a tutorial on multivariate gaussians, including variance vs covariance, and why multivariate Kalman filters are better than multiple univariate kalmans, since you get to take advantage of correlation.