OmniX-Space/xiaozhi-esp32
1
0
Fork 0
Code Issues Pull Requests Actions Releases Activity

add some code

Browse Source
This commit is contained in:
苏雨洛
2025-09-05 13:25:11 +08:00
parent 9ff0a99e7a
commit 3cf1229a85
8911 changed files with 2535396 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
managed_components/espressif__esp-dsp/examples/kalman/CMakeLists.txt Normal file
View File

@@ -0,0 +1,6 @@
# The following lines of boilerplate have to be in your project's
# CMakeLists in this exact order for cmake to work correctly
cmake_minimum_required(VERSION 3.5)
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(ekf13states)

149
managed_components/espressif__esp-dsp/examples/kalman/README.md Normal file
View File

@@ -0,0 +1,149 @@
# 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 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
```

1
managed_components/espressif__esp-dsp/examples/kalman/main/CMakeLists.txt Normal file
View File

@@ -0,0 +1 @@
idf_component_register(SRCS "ekf_imu13states_main.cpp")

196
managed_components/espressif__esp-dsp/examples/kalman/main/ekf_imu13states_main.cpp Normal file
View File

@@ -0,0 +1,196 @@
// Copyright 2020-2021 Espressif Systems (Shanghai) PTE LTD
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "dsp_platform.h"
#include "esp_log.h"
#include "esp_dsp.h"
#include "ekf_imu13states.h"
static const char *TAG = "main";
extern "C" void app_main();
// This example reproduce system with gyroscope, accelerometer, and magnetometer
// True gyroscope values will be transformed and applied to the rotation and reference measurements.
void app_main()
{
ekf_imu13states *ekf13 = new ekf_imu13states();
ekf13->Init();
ESP_LOGI(TAG, "Start Example.");
// Set up some initial values to emulate and calculate system values
int total_N = 3000;
// Pi value
float pi = std::atan(1) * 4;
// gyroscope bias error
float gyro_err_data[] = {0.1, 0.2, 0.3}; // static constatnt error
dspm::Mat gyro_err(gyro_err_data, 3, 1);
// Measurement noise covariance values for diagonal covariance matrix.
// For the real system these values could be adjusted!
// These calues depends on how noisy the measurement.
//
float R[10];
for (size_t i = 0; i < 10; i++) {
R[i] = 0.01;
}
// Reference vectors
float accel0_data[] = {0, 0, 1};
// In real system magnetometer vector will have different value and direction
// The EKF will calculate them. This value is used as initial state.
float magn0_data[] = {1, 0, 0};
dspm::Mat accel0(accel0_data, 3, 1);
dspm::Mat magn0(magn0_data, 3, 1);
float dt = 0.01;
dspm::Mat gyro_data(3, 1);
int count = 0;
// Initial rotation matrix
dspm::Mat Rm = dspm::Mat::eye(3);
dspm::Mat Re = dspm::Mat::eye(3);
gyro_err *= 1;
std::cout << "Gyro error: " << gyro_err.t() << std::endl;
std::cout << "Calibration phase started: " << std::endl;
for (size_t n = 1; n < total_N * 16; n++) {
if ((n % 1000) == 0) {
std::cout << "Loop " << n << " from " << total_N * 16;
std::cout << ", State data : " << ekf13->X.t();
}
//
// This part of the loop related to the system emulation
//
// Generate gyro values for system emulation
gyro_data *= 0; // reset gyro value
if ((n >= (total_N / 2)) && (n < total_N * 12)) {
gyro_data(0, 0) = 1 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
gyro_data(1, 0) = 2 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
gyro_data(2, 0) = 3 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
count++;
}
dspm::Mat gyro_sample = gyro_data + gyro_err;
gyro_data *= dt;
// Calculate rotation for the last time interval
Re = ekf::eul2rotm(gyro_data.data);
// Ally rotation to the system rotation matrix
Rm = Rm * Re;
// Convert rotation matrix to the system attitude quaternion
dspm::Mat attitude = ekf::rotm2quat(Rm);
// We have to rotate accel and magn to the opposite direction
dspm::Mat accel_data = Rm.t() * accel0;
dspm::Mat magn_data = Rm.t() * magn0;
dspm::Mat accel_norm = accel_data / accel_data.norm();
dspm::Mat magn_norm = magn_data / magn_data.norm();
//
// This part of the loop related to the real system
// Here gyro_sample values must be replaced by measured gyroscope values
// and accel_norm and magn_norm should be real measured accel and magn values
// The dt in this case should be real time difference in seconds between samples
// Fill the input control values with measured gyro values
float input_u[] = {gyro_sample(0, 0), gyro_sample(1, 0), gyro_sample(2, 0)};
// Process input values to new state
ekf13->Process(input_u, dt);
dspm::Mat q_norm(ekf13->X.data, 4, 1);
q_norm /= q_norm.norm();
// Correct state and calculate gyro and magnetometer values.
// Here accel_norm and magn_norm should be real measured accel and magn values
ekf13->UpdateRefMeasurementMagn(accel_norm.data, magn_norm.data, R);
}
std::cout << "Calibration phase finished." << std::endl << std::endl;
std::cout << "Regular calculation started:" << std::endl;
// Reset rotation nmatrix
Rm = dspm::Mat::eye(3);
Re = dspm::Mat::eye(3);
count = 0;
// Set initial state
ekf13->X(0, 0) = 1;
ekf13->X(0, 1) = 0;
ekf13->X(0, 2) = 0;
ekf13->X(0, 3) = 0;
for (size_t n = 1; n < total_N * 16; n++) {
if ((n % 1000) == 0) {
std::cout << "Loop " << n << " from " << total_N * 16;
std::cout << ", State data : " << ekf13->X.t();
}
//
// This part of the loop related to the system emulation
//
// Generate gyro values for system emulation
gyro_data *= 0; // reset gyro value
if ((n >= (total_N / 2)) && (n < total_N * 12)) {
gyro_data(0, 0) = 1 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
gyro_data(1, 0) = 2 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
gyro_data(2, 0) = 3 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
count++;
}
dspm::Mat gyro_sample = gyro_data + gyro_err;
gyro_data *= dt;
// Calculate rotation for the last time interval
Re = ekf::eul2rotm(gyro_data.data);
// Ally rotation to the system rotation matrix
Rm = Rm * Re;
// Convert rotation matrix to the system attitude quaternion
dspm::Mat attitude = ekf::rotm2quat(Rm);
// We have to rotate accel and magn to the opposite direction
dspm::Mat accel_data = Rm.t() * accel0;
dspm::Mat magn_data = Rm.t() * magn0;
dspm::Mat accel_norm = accel_data / accel_data.norm();
dspm::Mat magn_norm = magn_data / magn_data.norm();
//
// This part of the loop related to the real system
// Here gyro_sample values must be replaced by measured gyroscope values
// and accel_norm and magn_norm should be real measured accel and magn values
// The dt in this case should be real time difference in seconds between samples
// Fill the input control values with measured gyro values
float input_u[] = {gyro_sample(0, 0), gyro_sample(1, 0), gyro_sample(2, 0)};
// Process input values to new state
ekf13->Process(input_u, dt);
dspm::Mat q_norm(ekf13->X.data, 4, 1);
q_norm /= q_norm.norm();
// Correct state and calculate gyro and magnetometer values.
// Here accel_norm and magn_norm should be real measured accel and magn values
ekf13->UpdateRefMeasurement(accel_norm.data, magn_norm.data, R);
}
std::cout << "Final State data : " << ekf13->X.t();
dspm::Mat estimated_error(&ekf13->X.data[4], 3, 1);
std::cout << "Estimated error : " << estimated_error.t();
std::cout << "Difference between real and estimated errors : " << (gyro_err - estimated_error).t() << std::endl;
std::cout << "Expected Euler angels (degree) : " << (180 / pi * ekf::quat2eul(ekf::rotm2quat(Rm).data)).t();
std::cout << "Calculated Euler angels (degree) : " << (180 / pi * ekf::quat2eul(ekf13->X.data)).t() << std::endl;
}

4
managed_components/espressif__esp-dsp/examples/kalman/main/idf_component.yml Normal file
View File

@@ -0,0 +1,4 @@
dependencies:
espressif/esp-dsp:
override_path: "../../../"
version: "*"

22
managed_components/espressif__esp-dsp/examples/kalman/sdkconfig.defaults Normal file
View File

@@ -0,0 +1,22 @@
#
# DSP Library
#
# CONFIG_DSP_ANSI is not set
CONFIG_DSP_OPTIMIZED=y
# CONFIG_DSP_MAX_FFT_SIZE_512 is not set
# CONFIG_DSP_MAX_FFT_SIZE_1024 is not set
# CONFIG_DSP_MAX_FFT_SIZE_2048 is not set
CONFIG_DSP_MAX_FFT_SIZE_4096=y
# CONFIG_DSP_MAX_FFT_SIZE_8192 is not set
# CONFIG_DSP_MAX_FFT_SIZE_16384 is not set
# CONFIG_DSP_MAX_FFT_SIZE_32768 is not set
CONFIG_DSP_MAX_FFT_SIZE=4096
# end of DSP Library
#
# ESP System Settings
#
CONFIG_PARTITION_TABLE_OFFSET=0x9000
CONFIG_ESP_INT_WDT=n
CONFIG_ESP_TASK_WDT=n
# end of ESP System Settings