Extended Kalman Filter Software Implementation - Sensor Fusion #4 - Phil's Lab #73
HTML-код
- Опубликовано: 3 июл 2024
- Extended Kalman Filter (EKF) implementation and practical considerations. Real-world, real-time implementation and demo on an STM32 microcontroller in C using accelerometer and gyroscope measurements.
Part 4 (final) of sensor fusion video series.
[SUPPORT]
Free trial of Altium Designer: www.altium.com/yt/philslab
PCBA from $0 (Free Setup, Free Stencil): jlcpcb.com/RHS
Patreon: / phils94
[LINKS]
Git: github.com/pms67
Sensor Fusion Part 3: • Extended Kalman Filter...
Sensor Fusion Part 2: • Complementary Filter -...
Sensor Fusion Part 1: • Accelerometers and Gyr...
IIR Filters: • IIR Filters - Theory a...
Tag-Connect SWD Probe: www.tag-connect.com/product/t...
Small Unmanned Aircraft (Book): uavbook.byu.edu/doku.php
Euler Angles: control.asu.edu/Classes/MMAE44... (from slide 17)
[TIMESTAMPS]
00:00 Introduction
00:21 Altium Designer Free Trial
00:44 JLCPCB and Design Files
01:06 Pre-Requisites
01:53 'Low-Level' Firmware Overview
07:00 Axis Re-Mapping
08:17 Calibration
09:42 Filtering Raw Measurements
12:12 EKF Algorithm Overview
14:11 EKF Initialisation
17:12 EKF Predict Step
19:26 Matlab/Octave Symbolic Toolbox
21:11 EKF Update Step
22:16 Setting EKF Parameters
23:26 Debug Set-up and Tag-Connect SWD Probe
24:05 Live Demonstration
26:29 Practical Considerations
ID: QIBvbJtYjWuHiTG0uCoK 
Наука
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.
One of my favorite topics! Well worth the wait! I've not found very much practical EKF discussion for free. Lots of theoretical stuff...
Thanks a lot, Isaac! :)
I waited a long time for this one since the last part of the series! Thank you!
You've actually got so quality stuff on your channel. Keep that up man!
Thank you!
Thank you! I'm waiting for this part 4 video. Currently, I have a thesis regarding PDR. These videos really help!!! Thank you!
Can you share the sample base program from the one you have discussed?
Outstanding suite of videos!
THIS GUY IS THE BEST ..WE ALL LOVE YOU PHIL ❤❤🙌🙌🙌
Thank you very much :)
I was waiting for this video to come . Finally the wait is over!
Thanks for waiting! :)
thank you just what I needed👍
An incredible lesson. Thanks
Thanks!
Thank you very much! I am doing now my thesis on this topic and I have learned from your videos more than from the 3 scientific papers I read😊
Glad to hear the videos have been helpful - good luck with your thesis!
Hello, Can you please give me your contact information? I am also doing my thesis on this topic and struggling to find a way.
Finally an end to this topic, been waiting for so long
Good things come to those who wait!!! 😜
it's a great job , thank you for the videos.
Thanks, Alihan!
Thank you so much ! Perfect.
You're welcome!
Very interesting!
very nice, as always
Thanks, Sumit.
Wow you’re amazing !!!!
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.
Thank you, Gretar! I hadn't planned on covering the UKF but possibly will show the error-state Kalman filter in a future video.
Brilliant !
Thanks!
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!
Nice trick using Octave to derive the jacobian
Thanks, Rupert!
yes, lets gooo
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.
Thank you, Phil! Great video series!
Are you planning to make videos for quaternion approach?
Thank you!
Although I've implemented quaternion-based EKFs in the past, I'm afraid I haven't got any video planned on that for the near future.
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!
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 for practical explanation of EKF in video! But following github link, I can't find sources from this video, am I do something wrong?
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
Should SPI be used instead of I2C for mission-critical systems? If not, what would you recommend?
Yes, SPI is definitely preferred and pretty common. More advanced (and costly) IMUs will have more 'complicated'/robust interfaces.
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?
Hello, why there is a changing in sign for x and y coordinates in acceleration? The direction of x+ or y- is aligned with mpu
Phil, what about Madgwick filter? maybe also video?
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!
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.
Hello, How do I get the yaw angle?
Thanks!
Do you have recommandations on resource for a more advanced model ?
Thanks, Maxime. For aircraft in particular, I can highly recommend the book 'Aircraft Control and Simulation', which covers far more advanced models.
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.
@@PhilsLabwhat bus would you use instead?
@@PavloD9Probably SPI, as he did on the first version of the little brain board
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)
Great lecture! The following Github link doesn't take me to the source code, or did I make any mistakes?
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.
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?
Hi Phil, Nice contribution. Is source code available for this?
One other source of errors is that the sensors are not necessarily orthogonal.
Can you release the source code ?, I did not find it in your GitHub.
Is there any discord community for embedded systems and hardware Designing? I would love to join.
Why shouldn't the I2C be used for mission critical applications?
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.
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 :)
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.
great job man, I can't find the code on your git hub, is it there?
Thanks, Michele - check out the Hades repo, should be there.
wait, what? why not use I2C for "mission critical" functions?
Why not use I2C for mission critical systems?
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.
brother how did you learn stm32 programming ,please share me some information, book,pdf or something like that
can i get this stm32 code ? do share it man its so usefull for many.
Phil - Where you getting your ST micro's ?
LCSC mainly. Mouser has some less-powerful ones in stock.
@@PhilsLab Thanks matey, great video by the way appreciate the reply.
Wait you designed that whole module by yourself? Have you ever used ARIN429 protocol? These videos are godsent tbh thanks
Thank you!
Yeah, the hardware is my own design. Never worked with ARINC429 I'm afraid!
@@PhilsLab thanks for getting back. It’s amazing what the mind can do. I binge your videos, religiously. Cheers again.
Thank you very much, Andrew!
Is code available for this?