This example emulate system with IMU sensors and show how to use Extended Kalman Filter (EKF), with 13 values states vector, to estimate gyroscope errors and calculate system attitude. Also, this example show how to use esp-dsp library to operate with matrices and vectors.
In real system, the emulated sensors values should be replace by the real sensors values. Then, in real system, a calibration phase should be implemented and after the calibration phase the state vector X and covariance matrix P should be saved and restored next time, when filter called. It will save time for initial phase.
This example does not require any special hardware, and can be run on any common development board.
Under Component Config ---> DSP Library ---> DSP Optimization, it's possible to choose either the optimized or ANSI implementation, to compare them.
Build the project and flash it to the board, then run monitor tool to view serial output (replace PORT with serial port name):
idf.py -p PORT flash monitor
(To exit the serial monitor, type Ctrl-].)
See the Getting Started Guide for full steps to configure and use ESP-IDF to build projects.
I (380) spi_flash: detected chip: gd
I (383) spi_flash: flash io: dio
W (387) spi_flash: Detected size(4096k) larger than the size in the binary image header(2048k). Using the size in the binary image header.
I (404) cpu_start: Starting scheduler on PRO CPU.
I (0) cpu_start: Starting scheduler on APP CPU.
I (413) main: Start Example.
Gyro error: 0.1 0.2 0.3
Calibration phase started:
Loop 1000 from 48000, State data : 0.998361 0.0152476 0.0211183 0.0509682 0.00463435 0.00919946 0.01352 0.998156 0.00619182 -0.000683098 -0.00117112 0.0063196 -0.000952147
Loop 2000 from 48000, State data : 0.941757 0.0877462 0.170681 0.276156 0.016951 0.0334337 0.0498731 0.998804 0.0162317 -0.00225174 0.00389746 0.0110905 -0.000489083
Loop 3000 from 48000, State data : 0.372216 0.24247 0.488788 0.750832 0.0323164 0.0642265 0.0962768 0.997295 0.0269348 -0.00481966 0.00605674 0.00779719 0.00494921
Loop 4000 from 48000, State data : 0.944725 0.0951798 0.165878 0.266308 0.0470155 0.0946294 0.141251 0.998213 0.0337875 -0.00704064 0.00422252 0.0124181 0.00485692
Loop 5000 from 48000, State data : 0.944287 0.102183 0.168344 0.263706 0.0597481 0.12037 0.179946 0.997498 0.0378795 -0.00841348 0.0053515 0.0104612 0.00666854
Loop 6000 from 48000, State data : 0.379137 0.258284 0.476853 0.74977 0.0697741 0.140876 0.210702 0.995523 0.0410914 -0.00911293 0.00510267 0.00764586 0.00913832
Loop 7000 from 48000, State data : 0.947048 0.112494 0.165382 0.251187 0.0773002 0.156661 0.233985 0.996358 0.0425222 -0.00994576 0.00353348 0.00969652 0.00849919
Loop 8000 from 48000, State data : 0.945556 0.120624 0.169212 0.250481 0.082995 0.16838 0.251493 0.995914 0.0433827 -0.0102827 0.0039165 0.00846988 0.00913964
Loop 9000 from 48000, State data : 0.381034 0.276875 0.4647 0.749805 0.0871785 0.177046 0.264439 0.995073 0.0441243 -0.0103565 0.00391002 0.0071649 0.00997719
Loop 10000 from 48000, State data : 0.946592 0.132375 0.168307 0.241068 0.0902326 0.183443 0.273873 0.995445 0.0443655 -0.0106197 0.00326065 0.00799655 0.00960479
Loop 11000 from 48000, State data : 0.944297 0.140946 0.172816 0.242015 0.0924658 0.188118 0.280807 0.995187 0.0445766 -0.0106806 0.00346742 0.00749049 0.00979064
Loop 12000 from 48000, State data : 0.378334 0.295555 0.452859 0.751285 0.0941005 0.191525 0.285886 0.994796 0.0447763 -0.0106511 0.0034986 0.00695604 0.0100697
Loop 13000 from 48000, State data : 0.944329 0.1532 0.172826 0.234315 0.0953075 0.194011 0.289567 0.994899 0.0448155 -0.0107384 0.00323858 0.00728781 0.00989103
Loop 14000 from 48000, State data : 0.941572 0.16194 0.177533 0.236008 0.0961574 0.195842 0.292257 0.994735 0.0448801 -0.0107422 0.00334282 0.0070798 0.00993131
Loop 15000 from 48000, State data : 0.373427 0.314041 0.441061 0.753256 0.0967899 0.197167 0.294234 0.994523 0.0449438 -0.0107112 0.00335898 0.00685221 0.0100213
Loop 16000 from 48000, State data : 0.941028 0.174338 0.177916 0.228952 0.0972752 0.198121 0.295664 0.994518 0.0449512 -0.0107403 0.0032445 0.00697761 0.00993463
Loop 17000 from 48000, State data : 0.937959 0.183145 0.18262 0.230959 0.0975883 0.198844 0.296697 0.994396 0.0449757 -0.0107339 0.0032963 0.00688409 0.00992845
Loop 18000 from 48000, State data : 0.3675 0.33233 0.429142 0.755207 0.0978324 0.199358 0.297465 0.994256 0.0450002 -0.0107104 0.00330113 0.00677742 0.00995297
Loop 19000 from 48000, State data : 0.937014 0.195546 0.183166 0.224089 0.0980371 0.199716 0.298023 0.994211 0.0450036 -0.0107164 0.00324275 0.00681997 0.00990755
Loop 20000 from 48000, State data : 0.933698 0.204372 0.187784 0.226223 0.0981422 0.200008 0.29842 0.994114 0.0450155 -0.0107065 0.00327025 0.00677405 0.00988484
Loop 21000 from 48000, State data : 0.361055 0.350426 0.417036 0.756916 0.0982358 0.200208 0.29872 0.99401 0.0450272 -0.0106858 0.00326787 0.00671759 0.0098834
Loop 22000 from 48000, State data : 0.932425 0.216717 0.188413 0.219358 0.0983318 0.200334 0.298938 0.993947 0.0450303 -0.0106798 0.00323017 0.00672916 0.00985372
Loop 23000 from 48000, State data : 0.928888 0.225543 0.19291 0.221546 0.0983561 0.200458 0.299082 0.993866 0.0450371 -0.0106664 0.00324701 0.00670354 0.00982584
Loop 24000 from 48000, State data : 0.354297 0.36833 0.404722 0.758292 0.0983915 0.200535 0.299203 0.993773 0.045044 -0.0106443 0.00324109 0.00666712 0.00981608
Loop 25000 from 48000, State data : 0.927316 0.237803 0.19359 0.214612 0.0984453 0.200572 0.299291 0.993709 0.0450468 -0.0106303 0.00321085 0.00666748 0.00979369
Loop 26000 from 48000, State data : 0.92357 0.246618 0.197954 0.216824 0.0984385 0.200632 0.299342 0.993629 0.0450514 -0.0106123 0.00322403 0.00665109 0.00976504
Loop 27000 from 48000, State data : 0.347305 0.386034 0.392194 0.759303 0.0984521 0.200663 0.299393 0.993546 0.0450564 -0.0105864 0.00321847 0.00662376 0.00975319
Loop 28000 from 48000, State data : 0.92171 0.258777 0.198672 0.209796 0.0984898 0.200666 0.299433 0.993475 0.045059 -0.0105663 0.00319366 0.00662077 0.00973562
Loop 29000 from 48000, State data : 0.917761 0.267574 0.202895 0.212019 0.0984714 0.2007 0.299446 0.9934 0.0450631 -0.0105437 0.00320563 0.0066091 0.00970881
Loop 30000 from 48000, State data : 0.340113 0.403531 0.379459 0.759933 0.0984762 0.200711 0.299466 0.993322 0.0450673 -0.0105132 0.00320031 0.00658594 0.00969804
Loop 31000 from 48000, State data : 0.915619 0.279623 0.203648 0.204891 0.0985076 0.2007 0.299484 0.993253 0.0450696 -0.0104878 0.00317637 0.00658294 0.00968329
Loop 32000 from 48000, State data : 0.91147 0.288396 0.207727 0.207121 0.0984838 0.200725 0.299481 0.99318 0.0450732 -0.0104599 0.00318786 0.00657435 0.00965871
Loop 33000 from 48000, State data : 0.332734 0.420812 0.366519 0.760177 0.0984844 0.20073 0.299492 0.993105 0.045077 -0.0104242 0.00318322 0.00655352 0.00964965
Loop 34000 from 48000, State data : 0.909049 0.300327 0.208511 0.199891 0.0985129 0.200714 0.299506 0.993034 0.0450794 -0.0103941 0.0031609 0.00655179 0.00963774
Loop 35000 from 48000, State data : 0.904704 0.309072 0.212442 0.202124 0.0984875 0.200736 0.299498 0.992959 0.0450829 -0.0103612 0.0031732 0.00654506 0.00961709
Loop 36000 from 48000, State data : 0.325179 0.437867 0.353384 0.760034 0.098487 0.200738 0.299504 0.992885 0.0450865 -0.0103208 0.00317079 0.00652607 0.00961171
Loop 37000 from 48000, State data : 0.325177 0.437848 0.353377 0.760049 0.0989011 0.200534 0.299617 0.992838 0.0450912 -0.0103039 0.00319877 0.00652725 0.00959415
Loop 38000 from 48000, State data : 0.325194 0.437821 0.353363 0.760064 0.099202 0.200388 0.299726 0.992838 0.0450926 -0.0102998 0.00320422 0.00652895 0.00959084
Loop 39000 from 48000, State data : 0.325211 0.437798 0.353354 0.760074 0.0994169 0.200278 0.299826 0.992816 0.045093 -0.0102979 0.00320263 0.0065278 0.00959847
Loop 40000 from 48000, State data : 0.325222 0.437784 0.353346 0.760081 0.0995754 0.200199 0.299886 0.992816 0.0450925 -0.0102967 0.00320118 0.00652683 0.00960376
Loop 41000 from 48000, State data : 0.325231 0.437773 0.353342 0.760085 0.0996929 0.200142 0.299945 0.992816 0.0450925 -0.0102966 0.00320043 0.00652627 0.00960631
Loop 42000 from 48000, State data : 0.325238 0.437769 0.353336 0.760087 0.0997802 0.200119 0.299978 0.992816 0.0450913 -0.0102965 0.00320007 0.0065261 0.00960773
Loop 43000 from 48000, State data : 0.32524 0.437762 0.353331 0.760093 0.099842 0.200089 0.299979 0.992816 0.0450913 -0.0102961 0.00320001 0.00652608 0.00960857
Loop 44000 from 48000, State data : 0.325241 0.43776 0.353327 0.760095 0.099883 0.200059 0.299979 0.992816 0.045089 -0.0102953 0.00319975 0.00652622 0.00960868
Loop 45000 from 48000, State data : 0.325243 0.437759 0.353325 0.760096 0.0999138 0.200045 0.299979 0.992816 0.0450878 -0.0102956 0.0031996 0.00652593 0.00960985
Loop 46000 from 48000, State data : 0.325245 0.437756 0.353324 0.760097 0.0999355 0.200042 0.299979 0.992816 0.0450878 -0.0102959 0.00319972 0.0065261 0.00960944
Loop 47000 from 48000, State data : 0.325246 0.437757 0.353322 0.760098 0.0999504 0.200039 0.299979 0.992816 0.0450878 -0.0102959 0.00319952 0.00652634 0.00960984
Calibration phase finished.
Regular calculation started:
Loop 1000 from 48000, State data : 0.9996 6.68374e-06 -7.71055e-05 0.028269 0.0999599 0.199742 0.298758 0.992397 0.0506049 -0.00981295 0.00516875 0.00517689 0.0102406
Loop 2000 from 48000, State data : 0.95186 0.0747648 0.154942 0.253704 0.0997667 0.199899 0.298983 0.992397 0.0504132 -0.0098162 0.00510809 0.00522702 0.0102249
Loop 3000 from 48000, State data : 0.395338 0.237819 0.486861 0.741698 0.0994409 0.200091 0.299095 0.992397 0.0502891 -0.00981838 0.00506923 0.00525921 0.0102147
Loop 4000 from 48000, State data : 0.952465 0.0877289 0.155254 0.247002 0.0992076 0.200299 0.299271 0.992397 0.0502054 -0.00981973 0.00504271 0.00528111 0.0102076
Loop 5000 from 48000, State data : 0.950016 0.096521 0.160702 0.249654 0.0990023 0.200426 0.299306 0.992397 0.05013 -0.00982108 0.00501929 0.00530046 0.0102013
Loop 6000 from 48000, State data : 0.389808 0.256786 0.476033 0.745321 0.0988748 0.200476 0.299331 0.992397 0.050078 -0.00982208 0.0050032 0.00531378 0.0101971
Loop 7000 from 48000, State data : 0.950306 0.109383 0.161046 0.242937 0.0987883 0.200581 0.299444 0.992397 0.0500379 -0.00982289 0.0049906 0.00532416 0.0101938
Loop 8000 from 48000, State data : 0.947646 0.118155 0.166392 0.2456 0.0986931 0.200633 0.299434 0.992397 0.0499987 -0.00982366 0.00497852 0.00533416 0.0101903
Loop 9000 from 48000, State data : 0.383995 0.275557 0.464951 0.748623 0.0986473 0.200627 0.299425 0.992397 0.0499702 -0.00982415 0.00496994 0.00534126 0.0101879
Loop 10000 from 48000, State data : 0.94764 0.130939 0.166769 0.238794 0.0986203 0.200694 0.299509 0.992397 0.0499475 -0.00982454 0.004963 0.00534698 0.0101861
Loop 11000 from 48000, State data : 0.944771 0.139703 0.171999 0.241471 0.0985699 0.200714 0.299481 0.992397 0.0499252 -0.00982467 0.00495605 0.00535274 0.0101844
Loop 12000 from 48000, State data : 0.377949 0.294161 0.453622 0.751566 0.0985571 0.200685 0.299459 0.992397 0.0499092 -0.00982467 0.00495124 0.0053567 0.0101832
Loop 13000 from 48000, State data : 0.944475 0.152413 0.172408 0.234549 0.0985539 0.200736 0.299532 0.992397 0.0498962 -0.00982467 0.00494735 0.0053599 0.0101823
Loop 14000 from 48000, State data : 0.941398 0.16117 0.177516 0.237239 0.0985215 0.200743 0.299498 0.992397 0.0498826 -0.00982467 0.0049434 0.00536316 0.0101813
Loop 15000 from 48000, State data : 0.371694 0.312599 0.442052 0.754132 0.0985222 0.200704 0.299474 0.992397 0.049874 -0.00982467 0.00494084 0.0053653 0.0101808
Loop 16000 from 48000, State data : 0.940819 0.173801 0.177954 0.230187 0.0985277 0.20075 0.299544 0.992397 0.0498671 -0.00982467 0.00493887 0.00536698 0.0101804
Loop 17000 from 48000, State data : 0.937532 0.182552 0.182939 0.232897 0.0985023 0.200752 0.299507 0.992397 0.0498602 -0.00982467 0.0049367 0.0053688 0.0101799
Loop 18000 from 48000, State data : 0.365235 0.33087 0.43025 0.756316 0.0985084 0.200708 0.299482 0.992397 0.0498562 -0.00982467 0.00493555 0.0053698 0.0101796
Loop 19000 from 48000, State data : 0.936669 0.195102 0.183407 0.225717 0.0985175 0.200754 0.299549 0.992397 0.0498532 -0.00982467 0.00493474 0.00537051 0.0101794
Loop 20000 from 48000, State data : 0.933178 0.203841 0.188264 0.22844 0.0984953 0.200754 0.299511 0.992397 0.0498502 -0.00982467 0.00493366 0.00537141 0.010179
Loop 21000 from 48000, State data : 0.35858 0.348967 0.41822 0.758113 0.0985034 0.200709 0.299484 0.992397 0.0498495 -0.00982467 0.00493334 0.00537172 0.0101788
Loop 22000 from 48000, State data : 0.93203 0.216304 0.188763 0.221136 0.0985129 0.200755 0.299549 0.992397 0.0498497 -0.00982467 0.00493327 0.00537182 0.0101787
Loop 23000 from 48000, State data : 0.928336 0.225027 0.193488 0.22387 0.0984918 0.200754 0.29951 0.992397 0.0498497 -0.00982467 0.00493298 0.00537209 0.0101787
Loop 24000 from 48000, State data : 0.351735 0.366882 0.405969 0.759519 0.0985006 0.200707 0.299482 0.992397 0.0498508 -0.00982467 0.00493324 0.00537189 0.0101787
Loop 25000 from 48000, State data : 0.926905 0.237397 0.194017 0.216443 0.0985107 0.200755 0.299545 0.992397 0.0498526 -0.00982467 0.00493366 0.00537159 0.0101787
Loop 26000 from 48000, State data : 0.92301 0.246099 0.198608 0.219185 0.0984902 0.200753 0.299511 0.992397 0.0498537 -0.00982467 0.00493391 0.00537146 0.0101787
Loop 27000 from 48000, State data : 0.344708 0.384605 0.393502 0.760534 0.0984992 0.200706 0.299488 0.992397 0.0498556 -0.00982467 0.00493459 0.00537092 0.0101787
Loop 28000 from 48000, State data : 0.921297 0.258369 0.199166 0.211636 0.0985091 0.200757 0.299555 0.992397 0.0498579 -0.00982467 0.00493535 0.00537033 0.0101787
Loop 29000 from 48000, State data : 0.917204 0.267048 0.203621 0.214385 0.0984891 0.200754 0.29952 0.992397 0.0498601 -0.00982467 0.00493597 0.0053698 0.0101787
Loop 30000 from 48000, State data : 0.337501 0.402129 0.380825 0.761156 0.0984981 0.200706 0.299491 0.992397 0.0498626 -0.00982467 0.00493691 0.00536903 0.0101787
Loop 31000 from 48000, State data : 0.915208 0.279212 0.204208 0.206723 0.0985072 0.200758 0.299553 0.992397 0.0498654 -0.00982467 0.00493789 0.00536824 0.0101787
Loop 32000 from 48000, State data : 0.910918 0.287861 0.208526 0.209479 0.0984873 0.200754 0.299515 0.992397 0.0498676 -0.00982467 0.00493879 0.00536752 0.0101787
Loop 33000 from 48000, State data : 0.330116 0.419445 0.367945 0.761385 0.0984966 0.200704 0.299489 0.992397 0.0498701 -0.00982467 0.00493986 0.00536667 0.0101787
Loop 34000 from 48000, State data : 0.908641 0.299912 0.209141 0.201703 0.0985056 0.200757 0.299551 0.992397 0.0498728 -0.00982467 0.00494093 0.00536582 0.0101787
Loop 35000 from 48000, State data : 0.904158 0.308528 0.213318 0.204462 0.0984862 0.200752 0.299518 0.992397 0.0498751 -0.00982467 0.00494188 0.00536507 0.0101787
Loop 36000 from 48000, State data : 0.322561 0.436541 0.354869 0.761219 0.0984953 0.200702 0.299495 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 37000 from 48000, State data : 0.322549 0.436508 0.354868 0.761243 0.0989076 0.200506 0.299634 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 38000 from 48000, State data : 0.322568 0.436485 0.354851 0.761257 0.099208 0.200371 0.299726 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 39000 from 48000, State data : 0.322581 0.436466 0.354841 0.761267 0.0994207 0.200267 0.299799 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 40000 from 48000, State data : 0.322592 0.436454 0.354831 0.761274 0.099581 0.200208 0.299849 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 41000 from 48000, State data : 0.322601 0.436443 0.354826 0.761278 0.099701 0.20015 0.299901 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 42000 from 48000, State data : 0.322606 0.436435 0.35482 0.761283 0.0997844 0.200101 0.299931 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 43000 from 48000, State data : 0.322611 0.436429 0.35482 0.761285 0.0998457 0.200072 0.299961 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 44000 from 48000, State data : 0.322615 0.436425 0.354818 0.761286 0.0998883 0.200057 0.29999 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 45000 from 48000, State data : 0.322617 0.436423 0.354816 0.761287 0.099912 0.200042 0.300002 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 46000 from 48000, State data : 0.322617 0.436421 0.354815 0.761289 0.0999343 0.20003 0.300002 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Loop 47000 from 48000, State data : 0.322618 0.436421 0.354814 0.761289 0.0999444 0.20003 0.300002 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Final State data : 0.322618 0.436421 0.354814 0.761289 0.0999518 0.20003 0.300002 0.992397 0.0498778 -0.00982467 0.00494296 0.00536423 0.0101787
Estimated error : 0.0999518 0.20003 0.300002
Difference between real and estimated errors : 4.81904e-05 -2.99811e-05 -2.17557e-06
Expected Euler angels (degree) : -29.8215 64.9692 150.241
Calculated Euler angels (degree) : -35.1525 63.3067 156.167