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.