Huge respect. I'm an engineering student and you helped me see the clear in a forest of theories. I haven't been able to find such quality information anywhere, thank you so much.
Great video series! My master thesis was all about an error state space Kalman filter for the use on a multicopter, estimating the accelerometer and gyroscope errors and bias using different measurements. It’s really interesting to see that running on real hardware, rather than simulation. If you’re planning on doing a video on error state space, I‘d recommend using Simulink for the algorithms and Embedded Coder with the Hardware Support Packages for Arm Cortex-M.
Thanks for the video Phil! Currently I use a complementary filter for my flight control software and it works quite well. But one day I may go down this rabbit hole...
Here is some more infos if you really want to delve deep into the rabbit hole: - The EKF algorithm assumes the system's nonlinearity isn't too drastic, this might not hold for many dynamics flight environment. So an algorithm better suited for nonlinear system was developed, called Unscented Kalman Filter. This filter doesn't rely on linearization of the Jacobian matrix. - Euler's angles suffer from well-known drawback known as gimbal lock. Therefore quaternion is preferred to represent orientation. But unlike angles which can be estimated by adding angle rate, quaternion "adding" operation is multiplication. Therefore an algorithm called Multiplicative Extended Kalman Filter is developed to address this problem. - The error of many systems are usually Gaussian white noise, which is independent of the system dynamics. Therefore the behavior of the system's error is usually less complex than the system itself, for example: in the first video, Phil showed that despite the gyro rate function being nonlinear (using trigonometric operation), the error (gyro drift) appears to be linear . Thus instead of estimating the real value of the system, an algorithm calls Error State Kalman Filter instead opt to estimate the error of the system, which is subtracted with the measurements to yield the true value. - The stability and convergent speed of the Kalman filter depends largely on the covariance P and gain K, which stabilize when the observer converges. An algorithm called Invariant Kalman Filter aims to achieve better performance by quickly converge those two aforementioned variables to fixed value, thus provide robustness to the system. Those are some overview of the more advance approach to the problem of estimating attitude. Now down the internet rabbit hole you go :)
Thanks, Steve! In many situations a complementary filter is enough - definitely. Although diving into Kalman filters does open up some rather nice and interesting possibilities.
@@PhilsLab Hi Phil, I've read some material that use a method called Allan's variance to get the matrix P, Q and R for a particular sensors. I wonder whether you've used this method to find out P,Q,R matrix in the video?
@@minhhungnguyen7867 This particular rabbit hole has some very nice mathematics! Invariant filters take advantage of the matrix Lie group structure of rotation matrices (SO(3)) and unit quaternions (S3) among others. Firstly you can embed orthonormality constraints directly into the filter (i.e. no quaternion normalisation required etc.), but you can also derive log-linear error dynamics, guaranteeing convergence in some instances.
Amazing and very inspiring video. I studied this theory in my master's program where we only used Matlab and Simulink. It's nice to see hands-on example like this! It would be interesting to compare the Extended Kalman Filter to the Unscentend Kalman Filter. I'm not sure which one is more robust.
They're fairly comparable, the UKF is more robust when the linearisation used in the EKF is not a good approximation of the nonlinear function (measurement or propagation). The UKF instead calculates the posterior mean and covariance from a set of sigma points.
Fantastic video! My background is state estimation algorithms but I've always avoided deploying them to actual hardware, this is a fab tutorial on making the leap from Matlab/Simulink to C!
Thank you very much, Joe! Very glad to hear that. Also cool to see you answering comments here on EKFs/state estimation. Any chance you can share some of your work?
@@PhilsLab I'll have a couple of papers coming out in a few months that I'll happily share, my main research focus is in error-state and invariant filtering for navigation and tracking but I'm by no means an expert!
So in non sensor fusion applications of EKF, the prediction step is purely based on theoretical equations/models, and then the update step incorporates real measured data. In this sensor fusion example, why is the real gyroscope measured data not used in the update step? And why is it relevant to the theoretical prediction step? Doesn't this somewhat defeat the purpose of a Kalman Filter? Thankyou.
Thank you awsome topic and you made look it easy. But when I was watching and writing code at the same time it could be annoying. Could we reach the code inside the update function ??
Although I've used it maybe once or twice, I'm afraid I don't have the greatest knowledge of the Madgwick filter - so I'm afraid for now I'd have to pass on making a video on that, sorry!
Could you provide some background on the decision to use gyro readings in the predict step and not just include both gyro and accel data in the update step? I assume this is because it is difficult to come up with a better state transition function without knowing more about the system which is IMU is sensing.
Thanks for the Educational Videos Phil! In reference to the I2C MPU6050.h file, 4:23 has a skip over a portion of the MPU6050_Init( ), was hoping you could enlighten me as to what lines 54-60 were? I couldn't find this driver within any repo on your Github.
An interesting and well made explanation video - though naturally a bit tough to follow, even if you have learned about Kalman-filters already in the past. However I suggest that you really zoom into your code rather than showing the whole IDE. I am watching your videos on my regular tv, and in spite of quite a sizeable screen, it is impossible to read your code on the screen.
Thank you very much, Uwe. Yes, I'm sorry about that - it had slipped my mind to increase the code size. I'll make sure to have it larger in future videos.
Another question. EKF produces an estimate for the current state. Clearly some of those previous estimates will be inaccurate. Can you in light of current information revisit past estimates to get a more accurate estimate of past estimates? For example, a rolling update of the last 20 estimates including the current? If that makes sense?
Yet another masterpiece of a tutorial. I've implemented an EKF in a way similar to yours a few months ago and I've found that using CMSIS-DSP library's matrix operation functions goes a long way to simplifying the code and the biggest bonus, in my opinion, is the fact that larger matrices do not exponentially increase the code size - I wouldn't want to imagine what an 6x6 or a 9x9 EKF matrix inversion would look like when typed out in such a verbose way as shown in this video. Do you think that implementing a different integration method, say RK-4, would result in any meaningful improvement, especially if the sample time is relatively low (5ms and less)?
Generally Euler (1st Order) integration is more than fine and you won't see any meaningful results from using higher order routines, especially if you're at a relatively high frequency. In fact, the linearisation of the propagation and measurement functions in an EKF is only first-order anyway as we use the Jacobian.
Thank you very much. Indeed, the CMSIS-DSP library is very useful and I'm planning on making a video covering it (in particular for the FFT) in the near future. Yes, a 6x6 or 9x9 EKF matrix inversion typed out like this would look rather grim.. And thanks to Joe who's already answered your integration method question :)
Hi Phil! Great job as always!, I would like to know your opinion, I have seen that is possible to program a microcontroller with Matlab and implement all the filtering I want. Do you think is better that way? do I have more tools to work with? is this way more straightforward? Thank you very much!
Hi Phil. Can you explain why I²C should not be used for mission critical components as you describe it at 02:12? And does that also have something to do with ST's implementation on the F4 or just in general?
Hi Thomas, First there's the general speed-limitation of I2C. Ideally, we'd want to retrieve measurements at a higher rate. Then, I2C (especially with multiple devices on the bus) is prone to 'hang-ups', which require either a restart of the device or sending a number of clock pulses to the device. For configuration of a device, I2C is perfectly fine - I just wouldn't want to use it, for example, for retrieving measurements in a flight controller.
I’m not an electrical engineer so take my comment with a grain of salt. I hope Philip can answer this for you if time allows it. My take: Firstly, i2c is half duplex and serial protocol. Secondly, think about bandwidth and noise in the system. Mission critical system need to be robust taking into account redundancy and reliability. Look into CAN or MIL533. Hope this helps.
Isn't adding a low pass software filter the wrong thing to do? I thought since EKF was an "optimal filter" and the filter is just getting rid of noise that the EKF does its own filtering, without adding another layer of time delay to the readings?
Amazing video! I do have a question though Phil, with the update stage when calculating the kalman gain, what matrix would you need to put in for P in order to result in a 3x3 matrix with variables p0:p3? I am trying to implement a kalman filter that will be used for a rocket and have am able to do most of the sensor fusion required although this step has had me stumped
There is another version of Kalman filter. I think, it's called unscented Kalman filter (ukf). People consider this for highly non linear systems. Could be interesting to implement in MCU. Is an EKF always good, except the calculation of jacobians? What if the gyroscope parameters are slightly off?
Unscented and more generally Cubature Kalman Filters are popular as they remove the need for calculating Jacobians. Just throw them any measurement and propagation functions and they'll work! In terms of gyroscope measurements, they tend to have a lot less noise than accelerometers, hence why they are a complementary pairing in a complementary filter! Over time however they will drift due to integration of noise called angular walk and bias. We can actually add in bias terms into the state for the EKF/UKF and estimate it, removing the drift from the gyroscope measurements to correct for it. The Mahony Filter is an alternative to this that uses a PI controller in a quite elegant way.
Hey Phil, I'm currently learning to use the Kalman Filter on the MPU6050 before I port it to another gyro/accel sensor, since it's obsolete now. I'm doing a low-power IoT project that runs on battery. Apart from transmitting raw data using DMA, do u have any suggestions for reducing power consumption? (Unfortunately, I don't know the on-board Digital Motion Processor algorithms well enough for me to use it)
With I2c you can easily get a scenario where a device gets a bit wrong which causes the bus to hang, as if it were constantly acknowledging. The cause can be a spike on the line and to correct it you need to disconnect the power to the faulty device so it resets. A better bus will use some error checking to make sure the bits are correct.
