DSP_library_for_ESP-IDF_ESP-DSP_DSP_library

# Extended Kalman Filter 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. ## How to use example ### Hardware required This example does not require any special hardware, and can be run on any common development board. ### Configure the project Under Component Config ---> DSP Library ---> DSP Optimization, it's possible to choose either the optimized or ANSI implementation, to compare them. ### Build and flash 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. ## Example Output ``` 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