This week I got the extended Kalman filter working. We decided to use the filterpy library for the filter to save time in writing the filter, and focus on getting it to work. When I was starting, I ran into some issues with making sure I had the right dimensions for all the matrices. At first I was trying to do linear Kalman filter, but did it incorrectly, and realized that I used a measurement model wasn’t a constant. Since the measurement was nonlinear, I decided to work on the extended Kalman filter instead. The first time I tried to test the Kalman filter the positions it was estimating were very wrong, and realized that the equations I was using for the measurement model were incorrect. Once I fixed the equations, I ran into a few other issues where I was using the wrong variable names for certain things. After I fixed those last few bugs, we had a working extended Kalman filter.
Our current simulation doesn’t have any noise, so with perfect data the extended Kalman filter and nonlinear localization perform about the same.
Next week, Udit and I plan to discus the data the Shiva gathered, and I will work to incorporate that into the filter. Udit will be also adding noise to our simulation soon, and once he does that I’ll work on tuning the filter.