Tested against actual implementation

I tested the real data I recorded with an actual working implementation of an extended kalman filter, the one I’m roughly following as a guide for my own. I used ai a lot which worked pretty well, the code is very ugly but its just for testing so it doesn’t really matter. After my ai had got it working I went in and tuned the parameters for my dataset and it actually performed kinda worse than my own code! This really encouraged me because it means that my core code works and the errors are with the data processing. Next I think I’ll go through the gauntlet of calibrating my IMU which shouldn’t be too hard because I already did for a very similar project a while ago.

Improved performance with real data

Overall most of what I did here was bang my head against a wall trying to make it work. I made a lot of small changes but as you can see with the video still a lot to go. The biggest change I made was originally I used a NED or north east down corresponding to x y z and I switched over for ENU or east north up. I though originally I could just choose at a whim which one to use but different sensors use different reference frames and converting it over is more trouble than its worth. Now the output is a good bit better and I think I can improve it a lot by calibrating the sensor, currently the gyro outputs 3 deg/s when stationary. Next I will also be comparing against a much better kalman filter implementation in python to see how much of it is processing the raw data or the core algorithm.

Finally fixed all memory leaks

After running on the rp2040 again after I thought I fixed the memory leaks it still panicked with an out of memory error. So I went to take another look. The main problem is that all the basic matrix math functions allocated a new matrix for their returns. I had an idea of restructuring them to instead input another matrix to store the result and only return an error code. In practice this didn’t do much since I needed to allocate memory most of the time anyway. This did help a little bit but was probably more trouble than it was worth. Along the way of doing this I noticed a few more memory leaks and fixed them as I went. When I was done I got compile error after compile error but I eventually worked through all of them. But once the compiler errors went away it gave an output that was just slightly off. I narrowed it down to being in the math file and I sent my ai of to find the bug. After a long wait the error was simply that I forgot to uncomment some code when I was debugging the compiler errors. After poking around at a few more memory leaks it now gives the same output without losing any memory at all!

Tested with recorded data

Since the hardware I setup spat out gibberish when I first tried running kinetic on it I thought to record the raw imu output and test that on my computer instead. To do this I modified my rp2040 script to print the data to a serial output with a 10ms delay. I then asked my ai to make a script to record the data and save it to a csv file which it was able to do pretty well. On the kinetic side I made a new file that opens a csv file and pipes the data into kinetic which a little more help from ai. I ran it and to my surprise it actually outputted what I would expect. The picture below is the kinetic output from when the imu was sitting on my desk. I did run it again with a few different datasets I recorded and for some reason the angle never went above 1 deg which is a little weird and still wrong but definitely not the erratic output I first saw.

Fixed memory leaks (mostly)

This is a bit of a tangent from running on my rp2040 but it’s important. When I did run it on my board it threw an out of memory panic so I began to look for ways to fix that. I found valgrind which is a tool to check for memory leaks in c/c++ programs which was exactly what I need. Running it revealed that my test suite alone ate up ~600 kilobytes of extra memory which is very not good. I began to go through all the errors valgrind gave me and finding ways to free memory. I created a few functions to help with this, mainly for the core functions such as add/sub_matrix and mul_matrix, that run the base function but then free the first argument to make the code a little cleaner. In the end I was able to solve all but two memory leaks in the kinetic code. One I think is a false positive where the error is when the kalman filter state is updated. There were a few more in the simulation code. I tried to fix some but gave up since it doesn’t really mater if that leaks memory.

Got running on real hardware

After getting the IMU up and running I went and grabbed an oled display I had kicking around and plugged the whole thing into a breadboard. There is example oled driver code in the pico sdk so I took that and modified it to work for this. I then copied in my kinetic project and had to mess around a lot with cmake to get it to import right. I ended up making a new cmake file to help with this. But when it actually started running the output was nowhere near to correct and had an out of memory panic. I’ll probably figure out how to run it though a linter or something next.

Setting up to test on real hardware

So far the project looks good in my simulations, but now I want to move on to testing in real applications on real hardware. To this I grabbed some parts I had laying around including a custom devboard I made for a previous hackclub project. I am using the LSM9DS1 because it has a gyroscope, accelerometer, and a magnetometer in one chip and I happen to have one. I created a new git repo called imu-prototype to store all the files in. In there I set up a raspberry pi pico project and coded a very simple driver for the IMU.

Fixed Initial State Bug

As soon as I plugged the kalman filter to the new sin wave tests I noticed that the output was way off. I put off fixing it until first fixing the error function first. To find the bug I first tried asking my ai and it halucinated something that had nothing to do with the problem. I started to look for the bug myself and noticed that for the initial state the y axis had the wrong sign. When it should have had about 57 degrees it had -57 ish. So I went to the code that calculates that and couldn’t find an obvious bug. Since I couldn’t fix the existing code I replaced it with a different method to do the same thing. The old code calculated a rotation matrix with the accelerometer and magnetometer and the new code calculates the euler angles. I’m hoping that because it starts with a static state I won’t run into gimble lock but that’s a future me problem.

Improved error calculation

I noticed the error calculation didn’t seem to behave as expected so I did a little research and found an equation to find the angle between two quaternions and I’m using that angle as an error. I had to write two new functions, one to get the conjugate of a quaternion which was pretty simple and one to get the inverse of a quaternion which wasn’t to hard either.

Added new tests

Previously I only had one test that linearly integrates between two orientations. Now there are two more, one that’s x axis is a sin wave, and another where all axes are sin waves with an offset.

Put a lot of work into improving tests and simulations

I probably put too much effort into the simulations and tests and should work more on the actual code but having good tests is pretty nice. I added some simple alternate orientation algorithms, one that simply inegrates the gyro measurement and another that it a complementary filter. I revamped the simulation code to work with any number of algorithms as long as they have the same format. I then spent a while making a nice error sort of tui that changes based on how many algorithms are being used.

Found bug in simulation, now results are a lot better

After a little debugging I noticed a bug in the function in the simulation that calculates the rate of change between orientations used to simulate gyro measurements. After fixing that the kalman filter works almost perfectly from the graph!

ITS ALMOST WORKING! So much better that before.

After I wrote the code to abstract the kalman filter logic, there was still the rest of the algorithm to reimplement. I copied over what I could and for some of the functions I changed the way they worked. The guide I am following went through most of the calculations with the general matrix math and then at the end gave the full calculations to get the final matrix with single numbers. Following the spirit of my rewrite I decided to instead use the matrix math where I could. After this I hoked it up to my graphing program and it almost sorta kinda works! I have no idea what changed that makes it work but I’m not complaining.

Abstracted Kalman filter logic

I decided that since the code I already wrote wasn’t working, why not just try again? In all seriousness there was a few reasons for the rewrite, that other parts of the program might need their own kalman filter and it would be easier to understand and maintain but mostly I was sick of the code not working and not knowing why. So I found the basic equations for the extended kalman filter and went to implement that. At first I was using a struct of function pointers to get the input for the ekf (extended kalman filter) but was able to rewrite it to input static matrices instead which was way less clunky.

Still debugging but at least there’s a pretty graph

I hooked the kinetic output up to the graph program which only showed that the kinetic part doesn’t work.

Ditched ai code and wrote a better plotting script and a few bug fixes

After my ai couldn’t create the graph program I wanted and with calling python from c being so hard I went looking for alternatives. I found gnuplot which isn’t a c library but can be called fairly easily from c using pipes which I still don’t understand completely. I did find some code though that already works so I was able to modify it to what I wanted.

Used Hermes agent to write gnuplot program which still doesn’t work.

I’m going back an editing all my devlogs to more than just one line from this devlog to the one titled “Improved error calculation”.

I wanted to create a way to visualize what was going on. I initially tried to go with using matplotlib and calling the python from c. I thought this was something my ai could handle. It couldn’t. I don’t think it could make anything that could even compile. To the ai’s credit I was running it myself on my computer so it’s a lot dumber than most but it turned out to be harder than I thought.

Found out my inverse function works, the matrix I’m putting in is invalid

Getting Kalman Implementation Working

Project Overview

I’ve done a lot of work here on spare time and I got really behind on making journals so sorry for that. Since this is my first devlog I’ll start with an overview of this project. This is a sensor fusion algorithm (and a full flight controller if I have time) to obtain orientation, speed, direction, and position from raw sensor data.

What I Worked On

The bulk of the work here was getting the extended kalman filter for the imu working (which it still doesn’t) . I had a lot of trouble getting the initialization function working and after banging my head against a wall most of the problems were in the underlying matrix math functions and the weirdness of the matrix library I’m using. I also had a little side tangent of getting the initial orientation through an easier method but I eventually got it to work. To test all of this I wrote some code to generate fake gyrometer, accelerometer, and magnetometer data with some random rotations. Somewhere here I also decided to convert all doubles to floats and all size_t to uint_8 because this will be running on very minimal hardware. Once I got the init function working I then went on to the main loop function where I realized I did not know matrix math. A lot of my math functions were completely wrong or not the right ones. After fixing this I also did some restructuring to make the code flow better and I got the first half of the kalman filter mostly working. I then stumbled onto my atrocious matrix inversion function where I was just dividing all the elements under 1 which is definitely not how that works and after some research I added a few helper functions and that’s about the point I stopped, with the full inversion function still not working of course.