From 62431d8ddba7b74da61d527e3a24e74c69dda52a Mon Sep 17 00:00:00 2001 From: aneeshg5 Date: Sat, 1 Feb 2025 16:42:58 -0600 Subject: [PATCH 01/22] Create LookUp.csv --- MIDAS/src/gnc/LookUp.csv | 301 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 301 insertions(+) create mode 100644 MIDAS/src/gnc/LookUp.csv diff --git a/MIDAS/src/gnc/LookUp.csv b/MIDAS/src/gnc/LookUp.csv new file mode 100644 index 00000000..64091fe2 --- /dev/null +++ b/MIDAS/src/gnc/LookUp.csv @@ -0,0 +1,301 @@ +Mach Alpha CA Power-On CN CP +0.01 0 1.21363468 0 130.8434828 +0.02 0 1.059250667 0 130.8434828 +0.03 0 0.98198134 0 130.8434828 +0.04 0 0.932102339 0 130.8434828 +0.05 0 0.895957353 0 130.8434828 +0.06 0 0.867951263 0 130.8434828 +0.07 0 0.84527952 0 130.8434828 +0.08 0 0.826349122 0 130.8434828 +0.09 0 0.810174226 0 130.8434828 +0.1 0 0.796105211 0 130.8434828 +0.11 0 0.783692696 0 130.8434828 +0.12 0 0.772614241 0 130.8434828 +0.13 0 0.762630651 0 130.8434828 +0.14 0 0.753560431 0 130.8434828 +0.15 0 0.745262372 0 130.8434828 +0.16 0 0.737625023 0 130.8434828 +0.17 0 0.73055866 0 130.8434828 +0.18 0 0.723990282 0 130.8434828 +0.19 0 0.71785964 0 130.8434828 +0.2 0 0.712116363 0 130.8434828 +0.21 0 0.706718296 0 130.8434828 +0.22 0 0.701629416 0 130.8434828 +0.23 0 0.696819031 0 130.8434828 +0.24 0 0.69226057 0 130.8434828 +0.25 0 0.687931087 0 130.8434828 +0.26 0 0.684291729 0 130.8434828 +0.27 0 0.68083682 0 130.8434828 +0.28 0 0.677551341 0 130.8434828 +0.29 0 0.674421876 0 130.8434828 +0.3 0 0.671436546 0 130.8434828 +0.31 0 0.668584695 0 130.8434828 +0.32 0 0.665856819 0 130.8434828 +0.33 0 0.663244309 0 130.8434828 +0.34 0 0.660739414 0 130.8434828 +0.35 0 0.65833509 0 130.8434828 +0.36 0 0.656024994 0 130.8434828 +0.37 0 0.653803302 0 130.8434828 +0.38 0 0.651664687 0 130.8434828 +0.39 0 0.649604303 0 130.8434828 +0.4 0 0.647617643 0 130.8434828 +0.41 0 0.645700676 0 130.8434828 +0.42 0 0.643849546 0 130.8434828 +0.43 0 0.642060799 0 130.8434828 +0.44 0 0.640331201 0 130.8434828 +0.45 0 0.63865774 0 130.8434828 +0.46 0 0.637037674 0 130.8434828 +0.47 0 0.635468329 0 130.8434828 +0.48 0 0.633947375 0 130.8434828 +0.49 0 0.632472504 0 130.8434828 +0.5 0 0.631041685 0 130.8434828 +0.51 0 0.629652905 0 130.8434828 +0.52 0 0.628304284 0 130.8434828 +0.53 0 0.626994116 0 130.8434828 +0.54 0 0.625720828 0 130.8434828 +0.55 0 0.624482821 0 130.8434828 +0.56 0 0.62327875 0 130.8434828 +0.57 0 0.62210716 0 130.8434828 +0.58 0 0.620966804 0 130.8434828 +0.59 0 0.619354403 0 130.8434828 +0.6 0 0.617770878 0 130.8434828 +0.61 0 0.616961561 0 130.8434828 +0.62 0 0.616148054 0 130.8434828 +0.63 0 0.615356403 0 130.8434828 +0.64 0 0.614585797 0 130.8434828 +0.65 0 0.613835481 0 130.8434828 +0.66 0 0.613104659 0 130.8434828 +0.67 0 0.61239275 0 130.8434828 +0.68 0 0.611699077 0 130.8434828 +0.69 0 0.611022925 0 130.8434828 +0.7 0 0.6103638 0 130.8434828 +0.71 0 0.609721091 0 130.8434828 +0.72 0 0.609094272 0 130.8434828 +0.73 0 0.60848281 0 130.8434828 +0.74 0 0.607886229 0 130.8434828 +0.75 0 0.607304086 0 130.8434828 +0.76 0 0.606735837 0 130.8434828 +0.77 0 0.606181158 0 130.8434828 +0.78 0 0.605639576 0 130.8434828 +0.79 0 0.605110696 0 130.8434828 +0.8 0 0.60459417 0 130.8434828 +0.81 0 0.604089608 0 130.8434828 +0.82 0 0.603596698 0 130.8434828 +0.83 0 0.603115046 0 130.8434828 +0.84 0 0.60264439 0 130.8434828 +0.85 0 0.602184378 0 130.8434828 +0.86 0 0.601734704 0 130.8434828 +0.87 0 0.601295144 0 130.8434828 +0.88 0 0.600865377 0 130.8434828 +0.89 0 0.600445159 0 130.8434828 +0.9 0 0.600034218 0 130.8434828 +0.91 0 0.604114451 0 131.0711106 +0.92 0 0.616355149 0 131.2987383 +0.93 0 0.628985539 0 131.5263661 +0.94 0 0.635885271 0 131.7539938 +0.95 0 0.642785003 0 131.9816215 +0.96 0 0.649684735 0 132.2092493 +0.97 0 0.656584467 0 132.436877 +0.98 0 0.663484199 0 132.6645048 +0.99 0 0.670383931 0 132.8921325 +1 0 0.677283664 0 133.1197603 +0.01 2 1.215114659 0.805897084 128.4528665 +0.02 2 1.060542381 0.805897084 128.4528665 +0.03 2 0.983178827 0.805897084 128.4528665 +0.04 2 0.933239001 0.805897084 128.4528665 +0.05 2 0.897049937 0.805897084 128.4528665 +0.06 2 0.869009695 0.805897084 128.4528665 +0.07 2 0.846310305 0.805897084 128.4528665 +0.08 2 0.827356822 0.805897084 128.4528665 +0.09 2 0.811162201 0.805897084 128.4528665 +0.1 2 0.79707603 0.805897084 128.4528665 +0.11 2 0.784648378 0.805897084 128.4528665 +0.12 2 0.773556413 0.805897084 128.4528665 +0.13 2 0.763560649 0.805897084 128.4528665 +0.14 2 0.754479368 0.805897084 128.4528665 +0.15 2 0.74617119 0.805897084 128.4528665 +0.16 2 0.738524527 0.805897084 128.4528665 +0.17 2 0.731449547 0.805897084 128.4528665 +0.18 2 0.72487316 0.805897084 128.4528665 +0.19 2 0.718735042 0.805897084 128.4528665 +0.2 2 0.712984761 0.805897084 128.4528665 +0.21 2 0.707580111 0.805897084 128.4528665 +0.22 2 0.702485025 0.805897084 128.4528665 +0.23 2 0.697668774 0.805897084 128.4528665 +0.24 2 0.693104754 0.805897084 128.4528665 +0.25 2 0.688769992 0.805897084 128.4528665 +0.26 2 0.685126196 0.805897084 128.4528665 +0.27 2 0.681667073 0.805897084 128.4528665 +0.28 2 0.678377587 0.805897084 128.4528665 +0.29 2 0.675244307 0.805897084 128.4528665 +0.3 2 0.672255336 0.805897084 128.4528665 +0.31 2 0.669400007 0.805897084 128.4528665 +0.32 2 0.666668805 0.805897084 128.4528665 +0.33 2 0.664053109 0.805897084 128.4528665 +0.34 2 0.661545159 0.805897084 128.4528665 +0.35 2 0.659137903 0.805897084 128.4528665 +0.36 2 0.656824991 0.805897084 128.4528665 +0.37 2 0.65460059 0.805897084 128.4528665 +0.38 2 0.652459366 0.805897084 128.4528665 +0.39 2 0.65039647 0.805897084 128.4528665 +0.4 2 0.648407387 0.805897084 128.4528665 +0.41 2 0.646488082 0.805897084 128.4528665 +0.42 2 0.644634695 0.805897084 128.4528665 +0.43 2 0.642843766 0.805897084 128.4528665 +0.44 2 0.64111206 0.805897084 128.4528665 +0.45 2 0.639436558 0.805897084 128.4528665 +0.46 2 0.637814516 0.805897084 128.4528665 +0.47 2 0.636243257 0.805897084 128.4528665 +0.48 2 0.634720449 0.805897084 128.4528665 +0.49 2 0.633243779 0.805897084 128.4528665 +0.5 2 0.631811215 0.805897084 128.4528665 +0.51 2 0.630420742 0.805897084 128.4528665 +0.52 2 0.629070476 0.805897084 128.4528665 +0.53 2 0.62775871 0.805897084 128.4528665 +0.54 2 0.626483869 0.805897084 128.4528665 +0.55 2 0.625244353 0.805897084 128.4528665 +0.56 2 0.624038814 0.805897084 128.4528665 +0.57 2 0.622865794 0.805897084 128.4528665 +0.58 2 0.621724048 0.805897084 128.4528665 +0.59 2 0.620109681 0.805897084 128.4528665 +0.6 2 0.618524225 0.805897084 128.4528665 +0.61 2 0.617713921 0.805897084 128.4528665 +0.62 2 0.616899422 0.805897084 128.4528665 +0.63 2 0.616106805 0.805897084 128.4528665 +0.64 2 0.61533526 0.805897084 128.4528665 +0.65 2 0.614584029 0.805897084 128.4528665 +0.66 2 0.613852316 0.805897084 128.4528665 +0.67 2 0.613139538 0.805897084 128.4528665 +0.68 2 0.61244502 0.805897084 128.4528665 +0.69 2 0.611768043 0.805897084 128.4528665 +0.7 2 0.611108114 0.805897084 128.4528665 +0.71 2 0.610464622 0.805897084 128.4528665 +0.72 2 0.609837038 0.805897084 128.4528665 +0.73 2 0.609224831 0.805897084 128.4528665 +0.74 2 0.608627522 0.805897084 128.4528665 +0.75 2 0.608044669 0.805897084 128.4528665 +0.76 2 0.607475727 0.805897084 128.4528665 +0.77 2 0.606920372 0.805897084 128.4528665 +0.78 2 0.60637813 0.805897084 128.4528665 +0.79 2 0.605848604 0.805897084 128.4528665 +0.8 2 0.605331448 0.805897084 128.4528665 +0.81 2 0.604826271 0.805897084 128.4528665 +0.82 2 0.60433276 0.805897084 128.4528665 +0.83 2 0.603850521 0.805897084 128.4528665 +0.84 2 0.603379291 0.805897084 128.4528665 +0.85 2 0.602918718 0.805897084 128.4528665 +0.86 2 0.602468496 0.805897084 128.4528665 +0.87 2 0.6020284 0.805897084 128.4528665 +0.88 2 0.601598109 0.805897084 128.4528665 +0.89 2 0.601177378 0.805897084 128.4528665 +0.9 2 0.600765936 0.805897084 128.4528665 +0.91 2 0.604851144 0.80173249 128.6489201 +0.92 2 0.61710677 0.797567897 128.8449737 +0.93 2 0.629752561 0.793403304 129.0410273 +0.94 2 0.636660707 0.789238711 129.2370808 +0.95 2 0.643568854 0.785074117 129.4331344 +0.96 2 0.650477 0.780909524 129.629188 +0.97 2 0.657385146 0.776744931 129.8252416 +0.98 2 0.664293292 0.772580338 130.0212952 +0.99 2 0.671201438 0.768415744 130.2173488 +1 2 0.678109584 0.764251151 130.4134023 +0.01 4 1.219569061 1.730270476 126.3950588 +0.02 4 1.064430148 1.730270476 126.3950588 +0.03 4 0.986782992 1.730270476 126.3950588 +0.04 4 0.936660095 1.730270476 126.3950588 +0.05 4 0.900338368 1.730270476 126.3950588 +0.06 4 0.872195336 1.730270476 126.3950588 +0.07 4 0.849412733 1.730270476 126.3950588 +0.08 4 0.830389771 1.730270476 126.3950588 +0.09 4 0.814135782 1.730270476 126.3950588 +0.1 4 0.799997974 1.730270476 126.3950588 +0.11 4 0.787524764 1.730270476 126.3950588 +0.12 4 0.776392138 1.730270476 126.3950588 +0.13 4 0.766359731 1.730270476 126.3950588 +0.14 4 0.75724516 1.730270476 126.3950588 +0.15 4 0.748906526 1.730270476 126.3950588 +0.16 4 0.741231832 1.730270476 126.3950588 +0.17 4 0.734130916 1.730270476 126.3950588 +0.18 4 0.72753042 1.730270476 126.3950588 +0.19 4 0.721369801 1.730270476 126.3950588 +0.2 4 0.71559844 1.730270476 126.3950588 +0.21 4 0.710173979 1.730270476 126.3950588 +0.22 4 0.705060215 1.730270476 126.3950588 +0.23 4 0.700226308 1.730270476 126.3950588 +0.24 4 0.695645557 1.730270476 126.3950588 +0.25 4 0.691294905 1.730270476 126.3950588 +0.26 4 0.687637751 1.730270476 126.3950588 +0.27 4 0.684165948 1.730270476 126.3950588 +0.28 4 0.680864403 1.730270476 126.3950588 +0.29 4 0.677719637 1.730270476 126.3950588 +0.3 4 0.674719709 1.730270476 126.3950588 +0.31 4 0.671853913 1.730270476 126.3950588 +0.32 4 0.669112698 1.730270476 126.3950588 +0.33 4 0.666487414 1.730270476 126.3950588 +0.34 4 0.66397027 1.730270476 126.3950588 +0.35 4 0.66155419 1.730270476 126.3950588 +0.36 4 0.659232798 1.730270476 126.3950588 +0.37 4 0.657000243 1.730270476 126.3950588 +0.38 4 0.65485117 1.730270476 126.3950588 +0.39 4 0.652780712 1.730270476 126.3950588 +0.4 4 0.650784338 1.730270476 126.3950588 +0.41 4 0.648857996 1.730270476 126.3950588 +0.42 4 0.646997815 1.730270476 126.3950588 +0.43 4 0.645200321 1.730270476 126.3950588 +0.44 4 0.643462266 1.730270476 126.3950588 +0.45 4 0.641780622 1.730270476 126.3950588 +0.46 4 0.640152634 1.730270476 126.3950588 +0.47 4 0.638575616 1.730270476 126.3950588 +0.48 4 0.637047225 1.730270476 126.3950588 +0.49 4 0.635565142 1.730270476 126.3950588 +0.5 4 0.634127327 1.730270476 126.3950588 +0.51 4 0.632731756 1.730270476 126.3950588 +0.52 4 0.631376541 1.730270476 126.3950588 +0.53 4 0.630059966 1.730270476 126.3950588 +0.54 4 0.628780452 1.730270476 126.3950588 +0.55 4 0.627536391 1.730270476 126.3950588 +0.56 4 0.626326433 1.730270476 126.3950588 +0.57 4 0.625149114 1.730270476 126.3950588 +0.58 4 0.624003182 1.730270476 126.3950588 +0.59 4 0.622382897 1.730270476 126.3950588 +0.6 4 0.620791629 1.730270476 126.3950588 +0.61 4 0.619978354 1.730270476 126.3950588 +0.62 4 0.61916087 1.730270476 126.3950588 +0.63 4 0.618365347 1.730270476 126.3950588 +0.64 4 0.617590973 1.730270476 126.3950588 +0.65 4 0.616836989 1.730270476 126.3950588 +0.66 4 0.616102593 1.730270476 126.3950588 +0.67 4 0.615387203 1.730270476 126.3950588 +0.68 4 0.614690138 1.730270476 126.3950588 +0.69 4 0.61401068 1.730270476 126.3950588 +0.7 4 0.613348332 1.730270476 126.3950588 +0.71 4 0.612702481 1.730270476 126.3950588 +0.72 4 0.612072596 1.730270476 126.3950588 +0.73 4 0.611458144 1.730270476 126.3950588 +0.74 4 0.610858646 1.730270476 126.3950588 +0.75 4 0.610273657 1.730270476 126.3950588 +0.76 4 0.609702629 1.730270476 126.3950588 +0.77 4 0.609145238 1.730270476 126.3950588 +0.78 4 0.608601008 1.730270476 126.3950588 +0.79 4 0.608069541 1.730270476 126.3950588 +0.8 4 0.60755049 1.730270476 126.3950588 +0.81 4 0.607043461 1.730270476 126.3950588 +0.82 4 0.606548141 1.730270476 126.3950588 +0.83 4 0.606064134 1.730270476 126.3950588 +0.84 4 0.605591176 1.730270476 126.3950588 +0.85 4 0.605128915 1.730270476 126.3950588 +0.86 4 0.604677042 1.730270476 126.3950588 +0.87 4 0.604235333 1.730270476 126.3950588 +0.88 4 0.603803464 1.730270476 126.3950588 +0.89 4 0.603381191 1.730270476 126.3950588 +0.9 4 0.602968241 1.730270476 126.3950588 +0.91 4 0.607068425 1.721941289 126.5659665 +0.92 4 0.619368977 1.713612103 126.7368741 +0.93 4 0.632061126 1.705282916 126.9077818 +0.94 4 0.638994596 1.696953729 127.0786894 +0.95 4 0.645928067 1.688624543 127.2495971 +0.96 4 0.652861537 1.680295356 127.4205048 +0.97 4 0.659795007 1.67196617 127.5914124 +0.98 4 0.666728477 1.663636983 127.7623201 +0.99 4 0.673661947 1.655307797 127.9332278 +1 4 0.680595417 1.64697861 128.1041354 From 448a731f26735b1abf33cc63eb69a725382f7d2a Mon Sep 17 00:00:00 2001 From: Muhammad Arshad <63872178+MuhammadArshad123@users.noreply.github.com> Date: Sat, 1 Feb 2025 16:59:02 -0600 Subject: [PATCH 02/22] converting ekf.py to ekf.cpp --- MIDAS/src/gnc/ekf.cpp | 141 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 138 insertions(+), 3 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index d8c275bd..566644a8 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -115,6 +115,77 @@ void Yessir::initialize(RocketSystems* args) { // set B (don't care about what's in B since we have no control input) B(2, 0) = -1; + + //once FSM > 9, after its LANDED + std::map moonburner_data = { + {0.083, 1333.469}, + {0.13, 1368.376}, + {0.249, 1361.395}, + {0.308, 1380.012}, + {0.403, 1359.068}, + {0.675, 1184.53}, + {1.018, 1072.826}, + {1.456, 996.029}, + {1.977, 958.794}, + {2.995, 914.578}, + {3.99, 856.399}, + {4.985, 781.929}, + {5.494, 730.732}, + {5.991, 679.534}, + {7.258, 542.231}, + {7.862, 463.107}, + {8.015, 456.125}, + {8.998, 330.458}, + {9.993, 207.118}, + {10.514, 137.303}, + {11.496, 34.908}, + {11.994, 0.0} + }; + + //before FSM = 9 + std::map O5500X_data = { + {0.009, 20.408}, + {0.044, 7112.245}, + {0.063, 6734.694}, + {0.078, 6897.959}, + {0.094, 6612.245}, + {0.109, 6765.306}, + {0.125, 6540.816}, + {0.147, 6581.633}, + {0.194, 6520.408}, + {0.35, 6795.918}, + {0.428, 7091.837}, + {0.563, 7285.714}, + {0.694, 7408.163}, + {0.988, 7581.633}, + {1.266, 7622.449}, + {1.491, 7724.49}, + {1.581, 7653.061}, + {1.641, 7540.816}, + {1.684, 7500.0}, + {1.716, 7336.735}, + {1.784, 7224.49}, + {1.938, 6785.714}, + {2.138, 6326.531}, + {2.491, 5897.959}, + {2.6, 5704.082}, + {2.919, 3540.816}, + {3.022, 3408.163}, + {3.138, 2887.755}, + {3.3, 2234.694}, + {3.388, 1673.469}, + {3.441, 1489.796}, + {3.544, 1418.367}, + {3.609, 1295.918}, + {3.688, 816.327}, + {3.778, 653.061}, + {3.819, 581.633}, + {3.853, 489.796}, + {3.897, 285.714}, + {3.981, 20.408}, + {3.997, 0.0} + }; + } /** @@ -133,20 +204,84 @@ Eigen::Matrix Yessir::BodyToGlobal(euler_t angles, Eigen::Matrix xdot = Eigen::Matrix::Zero(); - xdot(0, 1) = x_k(1, 0); Orientation orientation = args->rocket_data.orientation.getRecent(); euler_t angles = orientation.getEuler(); + Eigen::Matrix gravity = Eigen::Matrix::Zero(); + gravity(2, 0) = -9.81; + world_accel = BodyToGlobal(angles, init_accel) + gravity; + + float w_x = world_accel(0, 0) * dt; + float w_y = world_accel(1, 0) * dt; + float w_z = world_accel(2, 0) * dt; + + float J_x = 1/2 * m * r**2 + float J_y = 1/3 * m * h**2 + 1/4 * m * r**2 + float J_z = J_y; + + float Fax = 0; + float Fay = 0; + float Faz = 0; + +// Fax = -0.5*rho*(vel_mag**2)*float(Ca)*(np.pi*r**2) - world_accel = BodyToGlobal(angles, init_accel) ; + Fg_body = R.inverse() * gravity; + Fgx = Fg_body[0]; + Fgy = Fg_body[1]; + Fgz = Fg_body[2]; + T = getThrust(); + + xdot << vel_x, + (Fax + Ftx + Fgx) / m - (w_y * vel_z - w_z * vel_y), + 1.0, + + vel_y, + (Fay + Fty + Fgy) / m - (w_z * vel_x - w_x * vel_y), + 1.0, + + vel_z, + (Faz + Ftz + Fgz) / m - (w_x * vel_y - w_y * vel_x), + 1.0; + + for (int i = 0; i < 3; i++) { + xdot(3 * i, 3 * i + 1) = xdot(); + xdot(3 * i, 3 * i + 2) = (s_dt * s_dt) / 2; + xdot(3 * i + 1, 3 * i + 2) = s_dt; + + F_mat(3 * i, 3 * i) = 1; + F_mat(3 * i + 1, 3 * i + 1) = 1; + F_mat(3 * i + 2, 3 * i + 2) = 1; + } + x_priori = (F_mat * x_k); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } +Eigen::Matrix Yessir:getThrust(euler_t angles, FSMState FSM_state) { + Eigen::Matrix T; + + // interpolate thrust values (weighted avg) + + if(FSM > 9) { + thrust(0, 0) = 0; + thrust(1, 0) = 0; + thrust(2, 0) = 0; + } else if (FSM <= 9) { + thrust(0, 0) = T * cos(angles.pitch) * sin(angles.roll); + thrust(1, 0) = T * sin(angles.pitch); + thrust(2, 0) = T * cos(angles.pitch) * cos(angles.roll); + } + + // return 3x1 thrust vector + + return T; +} + /** * @brief Update Kalman Gain and state estimate with current sensor data * From dad2b8fdc28bdd870c831770572cbcaa23ba6f18 Mon Sep 17 00:00:00 2001 From: Shishir Date: Sat, 1 Feb 2025 21:26:20 -0600 Subject: [PATCH 03/22] rip yessir.cpp :( Co-authored-by: aneeshg5 Co-authored-by: divijgarg Co-authored-by: Keshav B --- MIDAS/src/gnc/LookUp.csv | 327 +++------------------------------- MIDAS/src/gnc/ekf.cpp | 369 +++++++++++++++++++++++---------------- MIDAS/src/gnc/ekf.h | 17 +- MIDAS/src/sensor_data.h | 6 + MIDAS/src/systems.cpp | 6 +- 5 files changed, 260 insertions(+), 465 deletions(-) diff --git a/MIDAS/src/gnc/LookUp.csv b/MIDAS/src/gnc/LookUp.csv index 64091fe2..cfd5768a 100644 --- a/MIDAS/src/gnc/LookUp.csv +++ b/MIDAS/src/gnc/LookUp.csv @@ -1,301 +1,26 @@ -Mach Alpha CA Power-On CN CP -0.01 0 1.21363468 0 130.8434828 -0.02 0 1.059250667 0 130.8434828 -0.03 0 0.98198134 0 130.8434828 -0.04 0 0.932102339 0 130.8434828 -0.05 0 0.895957353 0 130.8434828 -0.06 0 0.867951263 0 130.8434828 -0.07 0 0.84527952 0 130.8434828 -0.08 0 0.826349122 0 130.8434828 -0.09 0 0.810174226 0 130.8434828 -0.1 0 0.796105211 0 130.8434828 -0.11 0 0.783692696 0 130.8434828 -0.12 0 0.772614241 0 130.8434828 -0.13 0 0.762630651 0 130.8434828 -0.14 0 0.753560431 0 130.8434828 -0.15 0 0.745262372 0 130.8434828 -0.16 0 0.737625023 0 130.8434828 -0.17 0 0.73055866 0 130.8434828 -0.18 0 0.723990282 0 130.8434828 -0.19 0 0.71785964 0 130.8434828 -0.2 0 0.712116363 0 130.8434828 -0.21 0 0.706718296 0 130.8434828 -0.22 0 0.701629416 0 130.8434828 -0.23 0 0.696819031 0 130.8434828 -0.24 0 0.69226057 0 130.8434828 -0.25 0 0.687931087 0 130.8434828 -0.26 0 0.684291729 0 130.8434828 -0.27 0 0.68083682 0 130.8434828 -0.28 0 0.677551341 0 130.8434828 -0.29 0 0.674421876 0 130.8434828 -0.3 0 0.671436546 0 130.8434828 -0.31 0 0.668584695 0 130.8434828 -0.32 0 0.665856819 0 130.8434828 -0.33 0 0.663244309 0 130.8434828 -0.34 0 0.660739414 0 130.8434828 -0.35 0 0.65833509 0 130.8434828 -0.36 0 0.656024994 0 130.8434828 -0.37 0 0.653803302 0 130.8434828 -0.38 0 0.651664687 0 130.8434828 -0.39 0 0.649604303 0 130.8434828 -0.4 0 0.647617643 0 130.8434828 -0.41 0 0.645700676 0 130.8434828 -0.42 0 0.643849546 0 130.8434828 -0.43 0 0.642060799 0 130.8434828 -0.44 0 0.640331201 0 130.8434828 -0.45 0 0.63865774 0 130.8434828 -0.46 0 0.637037674 0 130.8434828 -0.47 0 0.635468329 0 130.8434828 -0.48 0 0.633947375 0 130.8434828 -0.49 0 0.632472504 0 130.8434828 -0.5 0 0.631041685 0 130.8434828 -0.51 0 0.629652905 0 130.8434828 -0.52 0 0.628304284 0 130.8434828 -0.53 0 0.626994116 0 130.8434828 -0.54 0 0.625720828 0 130.8434828 -0.55 0 0.624482821 0 130.8434828 -0.56 0 0.62327875 0 130.8434828 -0.57 0 0.62210716 0 130.8434828 -0.58 0 0.620966804 0 130.8434828 -0.59 0 0.619354403 0 130.8434828 -0.6 0 0.617770878 0 130.8434828 -0.61 0 0.616961561 0 130.8434828 -0.62 0 0.616148054 0 130.8434828 -0.63 0 0.615356403 0 130.8434828 -0.64 0 0.614585797 0 130.8434828 -0.65 0 0.613835481 0 130.8434828 -0.66 0 0.613104659 0 130.8434828 -0.67 0 0.61239275 0 130.8434828 -0.68 0 0.611699077 0 130.8434828 -0.69 0 0.611022925 0 130.8434828 -0.7 0 0.6103638 0 130.8434828 -0.71 0 0.609721091 0 130.8434828 -0.72 0 0.609094272 0 130.8434828 -0.73 0 0.60848281 0 130.8434828 -0.74 0 0.607886229 0 130.8434828 -0.75 0 0.607304086 0 130.8434828 -0.76 0 0.606735837 0 130.8434828 -0.77 0 0.606181158 0 130.8434828 -0.78 0 0.605639576 0 130.8434828 -0.79 0 0.605110696 0 130.8434828 -0.8 0 0.60459417 0 130.8434828 -0.81 0 0.604089608 0 130.8434828 -0.82 0 0.603596698 0 130.8434828 -0.83 0 0.603115046 0 130.8434828 -0.84 0 0.60264439 0 130.8434828 -0.85 0 0.602184378 0 130.8434828 -0.86 0 0.601734704 0 130.8434828 -0.87 0 0.601295144 0 130.8434828 -0.88 0 0.600865377 0 130.8434828 -0.89 0 0.600445159 0 130.8434828 -0.9 0 0.600034218 0 130.8434828 -0.91 0 0.604114451 0 131.0711106 -0.92 0 0.616355149 0 131.2987383 -0.93 0 0.628985539 0 131.5263661 -0.94 0 0.635885271 0 131.7539938 -0.95 0 0.642785003 0 131.9816215 -0.96 0 0.649684735 0 132.2092493 -0.97 0 0.656584467 0 132.436877 -0.98 0 0.663484199 0 132.6645048 -0.99 0 0.670383931 0 132.8921325 -1 0 0.677283664 0 133.1197603 -0.01 2 1.215114659 0.805897084 128.4528665 -0.02 2 1.060542381 0.805897084 128.4528665 -0.03 2 0.983178827 0.805897084 128.4528665 -0.04 2 0.933239001 0.805897084 128.4528665 -0.05 2 0.897049937 0.805897084 128.4528665 -0.06 2 0.869009695 0.805897084 128.4528665 -0.07 2 0.846310305 0.805897084 128.4528665 -0.08 2 0.827356822 0.805897084 128.4528665 -0.09 2 0.811162201 0.805897084 128.4528665 -0.1 2 0.79707603 0.805897084 128.4528665 -0.11 2 0.784648378 0.805897084 128.4528665 -0.12 2 0.773556413 0.805897084 128.4528665 -0.13 2 0.763560649 0.805897084 128.4528665 -0.14 2 0.754479368 0.805897084 128.4528665 -0.15 2 0.74617119 0.805897084 128.4528665 -0.16 2 0.738524527 0.805897084 128.4528665 -0.17 2 0.731449547 0.805897084 128.4528665 -0.18 2 0.72487316 0.805897084 128.4528665 -0.19 2 0.718735042 0.805897084 128.4528665 -0.2 2 0.712984761 0.805897084 128.4528665 -0.21 2 0.707580111 0.805897084 128.4528665 -0.22 2 0.702485025 0.805897084 128.4528665 -0.23 2 0.697668774 0.805897084 128.4528665 -0.24 2 0.693104754 0.805897084 128.4528665 -0.25 2 0.688769992 0.805897084 128.4528665 -0.26 2 0.685126196 0.805897084 128.4528665 -0.27 2 0.681667073 0.805897084 128.4528665 -0.28 2 0.678377587 0.805897084 128.4528665 -0.29 2 0.675244307 0.805897084 128.4528665 -0.3 2 0.672255336 0.805897084 128.4528665 -0.31 2 0.669400007 0.805897084 128.4528665 -0.32 2 0.666668805 0.805897084 128.4528665 -0.33 2 0.664053109 0.805897084 128.4528665 -0.34 2 0.661545159 0.805897084 128.4528665 -0.35 2 0.659137903 0.805897084 128.4528665 -0.36 2 0.656824991 0.805897084 128.4528665 -0.37 2 0.65460059 0.805897084 128.4528665 -0.38 2 0.652459366 0.805897084 128.4528665 -0.39 2 0.65039647 0.805897084 128.4528665 -0.4 2 0.648407387 0.805897084 128.4528665 -0.41 2 0.646488082 0.805897084 128.4528665 -0.42 2 0.644634695 0.805897084 128.4528665 -0.43 2 0.642843766 0.805897084 128.4528665 -0.44 2 0.64111206 0.805897084 128.4528665 -0.45 2 0.639436558 0.805897084 128.4528665 -0.46 2 0.637814516 0.805897084 128.4528665 -0.47 2 0.636243257 0.805897084 128.4528665 -0.48 2 0.634720449 0.805897084 128.4528665 -0.49 2 0.633243779 0.805897084 128.4528665 -0.5 2 0.631811215 0.805897084 128.4528665 -0.51 2 0.630420742 0.805897084 128.4528665 -0.52 2 0.629070476 0.805897084 128.4528665 -0.53 2 0.62775871 0.805897084 128.4528665 -0.54 2 0.626483869 0.805897084 128.4528665 -0.55 2 0.625244353 0.805897084 128.4528665 -0.56 2 0.624038814 0.805897084 128.4528665 -0.57 2 0.622865794 0.805897084 128.4528665 -0.58 2 0.621724048 0.805897084 128.4528665 -0.59 2 0.620109681 0.805897084 128.4528665 -0.6 2 0.618524225 0.805897084 128.4528665 -0.61 2 0.617713921 0.805897084 128.4528665 -0.62 2 0.616899422 0.805897084 128.4528665 -0.63 2 0.616106805 0.805897084 128.4528665 -0.64 2 0.61533526 0.805897084 128.4528665 -0.65 2 0.614584029 0.805897084 128.4528665 -0.66 2 0.613852316 0.805897084 128.4528665 -0.67 2 0.613139538 0.805897084 128.4528665 -0.68 2 0.61244502 0.805897084 128.4528665 -0.69 2 0.611768043 0.805897084 128.4528665 -0.7 2 0.611108114 0.805897084 128.4528665 -0.71 2 0.610464622 0.805897084 128.4528665 -0.72 2 0.609837038 0.805897084 128.4528665 -0.73 2 0.609224831 0.805897084 128.4528665 -0.74 2 0.608627522 0.805897084 128.4528665 -0.75 2 0.608044669 0.805897084 128.4528665 -0.76 2 0.607475727 0.805897084 128.4528665 -0.77 2 0.606920372 0.805897084 128.4528665 -0.78 2 0.60637813 0.805897084 128.4528665 -0.79 2 0.605848604 0.805897084 128.4528665 -0.8 2 0.605331448 0.805897084 128.4528665 -0.81 2 0.604826271 0.805897084 128.4528665 -0.82 2 0.60433276 0.805897084 128.4528665 -0.83 2 0.603850521 0.805897084 128.4528665 -0.84 2 0.603379291 0.805897084 128.4528665 -0.85 2 0.602918718 0.805897084 128.4528665 -0.86 2 0.602468496 0.805897084 128.4528665 -0.87 2 0.6020284 0.805897084 128.4528665 -0.88 2 0.601598109 0.805897084 128.4528665 -0.89 2 0.601177378 0.805897084 128.4528665 -0.9 2 0.600765936 0.805897084 128.4528665 -0.91 2 0.604851144 0.80173249 128.6489201 -0.92 2 0.61710677 0.797567897 128.8449737 -0.93 2 0.629752561 0.793403304 129.0410273 -0.94 2 0.636660707 0.789238711 129.2370808 -0.95 2 0.643568854 0.785074117 129.4331344 -0.96 2 0.650477 0.780909524 129.629188 -0.97 2 0.657385146 0.776744931 129.8252416 -0.98 2 0.664293292 0.772580338 130.0212952 -0.99 2 0.671201438 0.768415744 130.2173488 -1 2 0.678109584 0.764251151 130.4134023 -0.01 4 1.219569061 1.730270476 126.3950588 -0.02 4 1.064430148 1.730270476 126.3950588 -0.03 4 0.986782992 1.730270476 126.3950588 -0.04 4 0.936660095 1.730270476 126.3950588 -0.05 4 0.900338368 1.730270476 126.3950588 -0.06 4 0.872195336 1.730270476 126.3950588 -0.07 4 0.849412733 1.730270476 126.3950588 -0.08 4 0.830389771 1.730270476 126.3950588 -0.09 4 0.814135782 1.730270476 126.3950588 -0.1 4 0.799997974 1.730270476 126.3950588 -0.11 4 0.787524764 1.730270476 126.3950588 -0.12 4 0.776392138 1.730270476 126.3950588 -0.13 4 0.766359731 1.730270476 126.3950588 -0.14 4 0.75724516 1.730270476 126.3950588 -0.15 4 0.748906526 1.730270476 126.3950588 -0.16 4 0.741231832 1.730270476 126.3950588 -0.17 4 0.734130916 1.730270476 126.3950588 -0.18 4 0.72753042 1.730270476 126.3950588 -0.19 4 0.721369801 1.730270476 126.3950588 -0.2 4 0.71559844 1.730270476 126.3950588 -0.21 4 0.710173979 1.730270476 126.3950588 -0.22 4 0.705060215 1.730270476 126.3950588 -0.23 4 0.700226308 1.730270476 126.3950588 -0.24 4 0.695645557 1.730270476 126.3950588 -0.25 4 0.691294905 1.730270476 126.3950588 -0.26 4 0.687637751 1.730270476 126.3950588 -0.27 4 0.684165948 1.730270476 126.3950588 -0.28 4 0.680864403 1.730270476 126.3950588 -0.29 4 0.677719637 1.730270476 126.3950588 -0.3 4 0.674719709 1.730270476 126.3950588 -0.31 4 0.671853913 1.730270476 126.3950588 -0.32 4 0.669112698 1.730270476 126.3950588 -0.33 4 0.666487414 1.730270476 126.3950588 -0.34 4 0.66397027 1.730270476 126.3950588 -0.35 4 0.66155419 1.730270476 126.3950588 -0.36 4 0.659232798 1.730270476 126.3950588 -0.37 4 0.657000243 1.730270476 126.3950588 -0.38 4 0.65485117 1.730270476 126.3950588 -0.39 4 0.652780712 1.730270476 126.3950588 -0.4 4 0.650784338 1.730270476 126.3950588 -0.41 4 0.648857996 1.730270476 126.3950588 -0.42 4 0.646997815 1.730270476 126.3950588 -0.43 4 0.645200321 1.730270476 126.3950588 -0.44 4 0.643462266 1.730270476 126.3950588 -0.45 4 0.641780622 1.730270476 126.3950588 -0.46 4 0.640152634 1.730270476 126.3950588 -0.47 4 0.638575616 1.730270476 126.3950588 -0.48 4 0.637047225 1.730270476 126.3950588 -0.49 4 0.635565142 1.730270476 126.3950588 -0.5 4 0.634127327 1.730270476 126.3950588 -0.51 4 0.632731756 1.730270476 126.3950588 -0.52 4 0.631376541 1.730270476 126.3950588 -0.53 4 0.630059966 1.730270476 126.3950588 -0.54 4 0.628780452 1.730270476 126.3950588 -0.55 4 0.627536391 1.730270476 126.3950588 -0.56 4 0.626326433 1.730270476 126.3950588 -0.57 4 0.625149114 1.730270476 126.3950588 -0.58 4 0.624003182 1.730270476 126.3950588 -0.59 4 0.622382897 1.730270476 126.3950588 -0.6 4 0.620791629 1.730270476 126.3950588 -0.61 4 0.619978354 1.730270476 126.3950588 -0.62 4 0.61916087 1.730270476 126.3950588 -0.63 4 0.618365347 1.730270476 126.3950588 -0.64 4 0.617590973 1.730270476 126.3950588 -0.65 4 0.616836989 1.730270476 126.3950588 -0.66 4 0.616102593 1.730270476 126.3950588 -0.67 4 0.615387203 1.730270476 126.3950588 -0.68 4 0.614690138 1.730270476 126.3950588 -0.69 4 0.61401068 1.730270476 126.3950588 -0.7 4 0.613348332 1.730270476 126.3950588 -0.71 4 0.612702481 1.730270476 126.3950588 -0.72 4 0.612072596 1.730270476 126.3950588 -0.73 4 0.611458144 1.730270476 126.3950588 -0.74 4 0.610858646 1.730270476 126.3950588 -0.75 4 0.610273657 1.730270476 126.3950588 -0.76 4 0.609702629 1.730270476 126.3950588 -0.77 4 0.609145238 1.730270476 126.3950588 -0.78 4 0.608601008 1.730270476 126.3950588 -0.79 4 0.608069541 1.730270476 126.3950588 -0.8 4 0.60755049 1.730270476 126.3950588 -0.81 4 0.607043461 1.730270476 126.3950588 -0.82 4 0.606548141 1.730270476 126.3950588 -0.83 4 0.606064134 1.730270476 126.3950588 -0.84 4 0.605591176 1.730270476 126.3950588 -0.85 4 0.605128915 1.730270476 126.3950588 -0.86 4 0.604677042 1.730270476 126.3950588 -0.87 4 0.604235333 1.730270476 126.3950588 -0.88 4 0.603803464 1.730270476 126.3950588 -0.89 4 0.603381191 1.730270476 126.3950588 -0.9 4 0.602968241 1.730270476 126.3950588 -0.91 4 0.607068425 1.721941289 126.5659665 -0.92 4 0.619368977 1.713612103 126.7368741 -0.93 4 0.632061126 1.705282916 126.9077818 -0.94 4 0.638994596 1.696953729 127.0786894 -0.95 4 0.645928067 1.688624543 127.2495971 -0.96 4 0.652861537 1.680295356 127.4205048 -0.97 4 0.659795007 1.67196617 127.5914124 -0.98 4 0.666728477 1.663636983 127.7623201 -0.99 4 0.673661947 1.655307797 127.9332278 -1 4 0.680595417 1.64697861 128.1041354 +Mach,Alpha,CA Power-On,CN,CP +0.04,4,1.332142905,1.1827808455016526632575949416,60.826787196815171703275113218 +0.08,4,1.326587387,1.1827808455016526632575949416,60.826787196815171703275113218 +0.12,4,1.31558627,1.1827808455016526632575949416,60.826787196815171703275113218 +0.16,4,1.306063953,1.1827808455016526632575949416,60.826787196815171703275113218 +0.20,4,1.298117084,1.1827808455016526632575949416,60.826787196815171703275113218 +0.24,4,1.291411025,1.1827808455016526632575949416,60.826787196815171703275113218 +0.28,4,1.290279857,1.1827808455016526632575949416,60.826787196815171703275113218 +0.32,4,1.291431043,1.1827808455016526632575949416,60.826787196815171703275113218 +0.36,4,1.293170653,1.1827808455016526632575949416,60.826787196815171703275113218 +0.40,4,1.295385827,1.1827808455016526632575949416,60.826787196815171703275113218 +0.44,4,1.297991738,1.1827808455016526632575949416,60.826787196815171703275113218 +0.48,4,1.300924032,1.1827808455016526632575949416,60.826787196815171703275113218 +0.52,4,1.304132086,1.1827808455016526632575949416,60.826787196815171703275113218 +0.56,4,1.309039395,1.1827808455016526632575949416,60.826787196815171703275113218 +0.60,4,1.314605487,1.1827808455016526632575949416,60.826787196815171703275113218 +0.64,4,1.330699437,1.1827808455016526632575949416,60.826787196815171703275113218 +0.68,4,1.346695167,1.1827808455016526632575949416,60.826787196815171703275113218 +0.72,4,1.362693183,1.1827808455016526632575949416,60.826787196815171703275113218 +0.76,4,1.378693074,1.1827808455016526632575949416,60.826787196815171703275113218 +0.80,4,1.394695194,1.1827808455016526632575949416,60.826787196815171703275113218 +0.84,4,1.41069913,1.1827808455016526632575949416,60.826787196815171703275113218 +0.88,4,1.426705046,1.1827808455016526632575949416,60.826787196815171703275113218 +0.92,4,1.473732816,1.218959468,60.91848997 +0.96,4,1.582395672,1.291316713,61.10189551 +1.00,4,1.681494886,1.363673958,61.28530105 diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 566644a8..40bc8882 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,8 +1,9 @@ #include "ekf.h" #include "finite-state-machines/fsm_states.h" -Yessir::Yessir() : KalmanFilter() { +EKF::EKF() : KalmanFilter() { state = KalmanData(); + initializeAerodynamicData(); } /** @@ -24,9 +25,86 @@ Yessir::Yessir() : KalmanFilter() { * */ -void Yessir::initialize(RocketSystems* args) { +const float pi = 3.14159268; +const float rho = 1.225; +const float r = 0.03935;//meters +const float height_full = 2.34; //height of rocket Full Stage - 2.34 m +const float height_sustainer = 1.34; +const float mass_full = 7.57; //kg Sustainer + Booster +const float mass_sustainer = 4.08; //kg Sustainer +// Moonburner motor +const std::map moonburner_data = { + {0.083, 1333.469}, + {0.13, 1368.376}, + {0.249, 1361.395}, + {0.308, 1380.012}, + {0.403, 1359.068}, + {0.675, 1184.53}, + {1.018, 1072.826}, + {1.456, 996.029}, + {1.977, 958.794}, + {2.995, 914.578}, + {3.99, 856.399}, + {4.985, 781.929}, + {5.494, 730.732}, + {5.991, 679.534}, + {7.258, 542.231}, + {7.862, 463.107}, + {8.015, 456.125}, + {8.998, 330.458}, + {9.993, 207.118}, + {10.514, 137.303}, + {11.496, 34.908}, + {11.994, 0.0} +}; + +// Booster motor +std::map O5500X_data = { + {0.009, 20.408}, + {0.044, 7112.245}, + {0.063, 6734.694}, + {0.078, 6897.959}, + {0.094, 6612.245}, + {0.109, 6765.306}, + {0.125, 6540.816}, + {0.147, 6581.633}, + {0.194, 6520.408}, + {0.35, 6795.918}, + {0.428, 7091.837}, + {0.563, 7285.714}, + {0.694, 7408.163}, + {0.988, 7581.633}, + {1.266, 7622.449}, + {1.491, 7724.49}, + {1.581, 7653.061}, + {1.641, 7540.816}, + {1.684, 7500.0}, + {1.716, 7336.735}, + {1.784, 7224.49}, + {1.938, 6785.714}, + {2.138, 6326.531}, + {2.491, 5897.959}, + {2.6, 5704.082}, + {2.919, 3540.816}, + {3.022, 3408.163}, + {3.138, 2887.755}, + {3.3, 2234.694}, + {3.388, 1673.469}, + {3.441, 1489.796}, + {3.544, 1418.367}, + {3.609, 1295.918}, + {3.688, 816.327}, + {3.778, 653.061}, + {3.819, 581.633}, + {3.853, 489.796}, + {3.897, 285.714}, + {3.981, 20.408}, + {3.997, 0.0} +}; + + +void EKF::initialize(RocketSystems* args) { Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); - float sum = 0; for (int i = 0; i < 30; i++) { @@ -58,16 +136,7 @@ void Yessir::initialize(RocketSystems* args) { x_k(3, 0) = 0; x_k(6, 0) = 0; - // set F - for (int i = 0; i < 3; i++) { - F_mat(3 * i, 3 * i + 1) = s_dt; - F_mat(3 * i, 3 * i + 2) = (s_dt * s_dt) / 2; - F_mat(3 * i + 1, 3 * i + 2) = s_dt; - - F_mat(3 * i, 3 * i) = 1; - F_mat(3 * i + 1, 3 * i + 1) = 1; - F_mat(3 * i + 2, 3 * i + 2) = 1; - } + F_mat.setZero(); // Initialize with zeros Q(0, 0) = pow(s_dt, 5) / 20; Q(0, 1) = pow(s_dt, 4) / 8; @@ -116,76 +185,6 @@ void Yessir::initialize(RocketSystems* args) { // set B (don't care about what's in B since we have no control input) B(2, 0) = -1; - //once FSM > 9, after its LANDED - std::map moonburner_data = { - {0.083, 1333.469}, - {0.13, 1368.376}, - {0.249, 1361.395}, - {0.308, 1380.012}, - {0.403, 1359.068}, - {0.675, 1184.53}, - {1.018, 1072.826}, - {1.456, 996.029}, - {1.977, 958.794}, - {2.995, 914.578}, - {3.99, 856.399}, - {4.985, 781.929}, - {5.494, 730.732}, - {5.991, 679.534}, - {7.258, 542.231}, - {7.862, 463.107}, - {8.015, 456.125}, - {8.998, 330.458}, - {9.993, 207.118}, - {10.514, 137.303}, - {11.496, 34.908}, - {11.994, 0.0} - }; - - //before FSM = 9 - std::map O5500X_data = { - {0.009, 20.408}, - {0.044, 7112.245}, - {0.063, 6734.694}, - {0.078, 6897.959}, - {0.094, 6612.245}, - {0.109, 6765.306}, - {0.125, 6540.816}, - {0.147, 6581.633}, - {0.194, 6520.408}, - {0.35, 6795.918}, - {0.428, 7091.837}, - {0.563, 7285.714}, - {0.694, 7408.163}, - {0.988, 7581.633}, - {1.266, 7622.449}, - {1.491, 7724.49}, - {1.581, 7653.061}, - {1.641, 7540.816}, - {1.684, 7500.0}, - {1.716, 7336.735}, - {1.784, 7224.49}, - {1.938, 6785.714}, - {2.138, 6326.531}, - {2.491, 5897.959}, - {2.6, 5704.082}, - {2.919, 3540.816}, - {3.022, 3408.163}, - {3.138, 2887.755}, - {3.3, 2234.694}, - {3.388, 1673.469}, - {3.441, 1489.796}, - {3.544, 1418.367}, - {3.609, 1295.918}, - {3.688, 816.327}, - {3.778, 653.061}, - {3.819, 581.633}, - {3.853, 489.796}, - {3.897, 285.714}, - {3.981, 20.408}, - {3.997, 0.0} - }; - } /** @@ -196,7 +195,7 @@ void Yessir::initialize(RocketSystems* args) { * it extrapolates the state at time n+1 based on the state at time n. */ -Eigen::Matrix Yessir::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { +Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { Eigen::Matrix3f roll, pitch, yaw; roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); @@ -204,82 +203,100 @@ Eigen::Matrix Yessir::BodyToGlobal(euler_t angles, Eigen::Matrix xdot = Eigen::Matrix::Zero(); - Orientation orientation = args->rocket_data.orientation.getRecent(); - + Velocity omega = orientation.getVelocity(); euler_t angles = orientation.getEuler(); - Eigen::Matrix gravity = Eigen::Matrix::Zero(); - gravity(2, 0) = -9.81; - world_accel = BodyToGlobal(angles, init_accel) + gravity; - - float w_x = world_accel(0, 0) * dt; - float w_y = world_accel(1, 0) * dt; - float w_z = world_accel(2, 0) * dt; - - float J_x = 1/2 * m * r**2 - float J_y = 1/3 * m * h**2 + 1/4 * m * r**2 + Eigen::Matrix gravity = Eigen::Matrix::Zero(); + gravity(0, 0) = -9.81; + Eigen::Matrix ang_pos = BodyToGlobal(angles, init_accel) + gravity; + + float m = mass_sustainer; + float h = height_sustainer; + if (fsm < FSMState::STATE_BURNOUT) { + m = mass_full; + h= height_full; + } + float w_x = omega.vx; + float w_y = omega.vy; + float w_z = omega.vz; + + float J_x = 0.5 * m * r * r; + float J_y = (1/3) * m * h * h + 0.25 * m * r * r; float J_z = J_y; + + float vel_mag_squared = x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0); - float Fax = 0; + float Fax = -0.5*rho*(vel_mag_squared)*float(Ca)*(pi*r*r); float Fay = 0; float Faz = 0; -// Fax = -0.5*rho*(vel_mag**2)*float(Ca)*(np.pi*r**2) + Eigen::Matrix Fg_body = R.inverse() * gravity; + float Fgx = Fg_body(0,0); float Fgy = Fg_body(1,0); float Fgz = Fg_body(2,0); + + Eigen::Matrix T = EKF::getThrust(stage_timestamp, ang_pos, fsm); + float Ftx = T(0, 0); float Fty = T(1, 0); float Ftz = T(2, 0); - Fg_body = R.inverse() * gravity; - - Fgx = Fg_body[0]; - Fgy = Fg_body[1]; - Fgz = Fg_body[2]; - T = getThrust(); - xdot << vel_x, - (Fax + Ftx + Fgx) / m - (w_y * vel_z - w_z * vel_y), + + xdot << x_k(1,0), + (Fax + Ftx + Fgx) / m - (w_y * x_k(7,0) - w_z * x_k(4,0)), 1.0, - vel_y, - (Fay + Fty + Fgy) / m - (w_z * vel_x - w_x * vel_y), + x_k(4,0), + (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), // this might not be right 1.0, - vel_z, - (Faz + Ftz + Fgz) / m - (w_x * vel_y - w_y * vel_x), + x_k(7,0), + (Faz + Ftz + Fgz) / m - (w_x * x_k(4,0) - w_y * x_k(1,0)), 1.0; - for (int i = 0; i < 3; i++) { - xdot(3 * i, 3 * i + 1) = xdot(); - xdot(3 * i, 3 * i + 2) = (s_dt * s_dt) / 2; - xdot(3 * i + 1, 3 * i + 2) = s_dt; + x_priori = (xdot * dt) + x_k; + EKF::set_F(dt, Ca, Cn, rho, w_x, w_y, w_z); + P_priori = (F_mat * P_k * F_mat.transpose()) + Q; +} - F_mat(3 * i, 3 * i) = 1; - F_mat(3 * i + 1, 3 * i + 1) = 1; - F_mat(3 * i + 2, 3 * i + 2) = 1; - } - x_priori = (F_mat * x_k); - P_priori = (F_mat * P_k * F_mat.transpose()) + Q; +float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) { + return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); } -Eigen::Matrix Yessir:getThrust(euler_t angles, FSMState FSM_state) { - Eigen::Matrix T; - // interpolate thrust values (weighted avg) +Eigen::Matrix EKF::getThrust(float timestamp, Eigen::Matrix angles, FSMState FSM_state) { + Eigen::Matrix thrust = Eigen::Matrix::Zero(); + if (FSM_state < FSMState::STATE_BURNOUT) { + // interpolate from O5500X_data + auto it = O5500X_data.lower_bound(time); + + if (it == moonburner_data.begin() || it == moonburner_data.end()) { + std::cout << "Timestep " << t << " is out of bounds for interpolation." << std::endl; + } else { + float x1 = it->first; + float y1 = it->second; + --it; + float x0 = it->first; + float y0 = it->second; + + float interpolatedValue = linearInterpolation(x0, y0, x1, y1, t); + std::cout << "Interpolated value at timestep " << t << " is " << interpolatedValue << std::endl; + } + } + else { + // interpolate from moonburner_data + + } - if(FSM > 9) { thrust(0, 0) = 0; thrust(1, 0) = 0; thrust(2, 0) = 0; - } else if (FSM <= 9) { - thrust(0, 0) = T * cos(angles.pitch) * sin(angles.roll); - thrust(1, 0) = T * sin(angles.pitch); - thrust(2, 0) = T * cos(angles.pitch) * cos(angles.roll); + } else if (FSM_state <= 9) { + thrust(0, 0) = thrust(0,0) * cos(angles(1,0)) * sin(angles(0,0)); + thrust(1, 0) = thrust(1,0) * sin(angles(1,0)); + thrust(2, 0) = thrust(2,0) * cos(angles(1,0)) * cos(angles(0,0)); } - - // return 3x1 thrust vector - - return T; + + return thrust; } /** @@ -290,7 +307,7 @@ Eigen::Matrix Yessir:getThrust(euler_t angles, FSMState FSM_state) * the new sensor data is. After updating the gain, the state estimate is updated. * */ -void Yessir::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { +void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { float sum = 0; float data[10]; @@ -360,15 +377,47 @@ void Yessir::update(Barometer barometer, Acceleration acceleration, Orientation * @param &orientation Current orientation * @param current_state Current FSM_state */ -void Yessir::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) { +void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) { if (FSM_state >= FSMState::STATE_IDLE) { + if (FSM_state != last_fsm) { + stage_timestamp = 0; + } + stage_timestamp += dt; setF(dt / 1000); setQ(dt / 1000, sd); - priori(); + priori(dt, orientation, FSM_state); update(barometer, acceleration, orientation, FSM_state); + FSMState last_fsm = FSM_state; + } +} +//LookUpMap + +void EKF::initializeAerodynamicData() { + std::ifstream file("LookUp.csv"); + std::string line; + + // Skip the header line + std::getline(file, line); + + while (std::getline(file, line)) { + std::istringstream iss(line); + std::string token; + std::vector row; + + while (std::getline(iss, token, ',')) { + row.push_back(std::stod(token)); + } + + if (row.size() >= 5) { + double mach = std::round(row[0] / 0.04) * 0.04; + aerodynamicData[mach] = std::make_tuple(row[2], row[3], row[4]); + } } } + + + /** * @brief Converts a vector in the body frame to the global frame * @@ -376,7 +425,7 @@ void Yessir::tick(float dt, float sd, Barometer &barometer, Acceleration acceler * @param body_vect Vector for rotation in the body frame * @return Eigen::Matrix Rotated vector in the global frame */ -Eigen::Matrix Yessir::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { +Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { Eigen::Matrix3f roll, pitch, yaw; roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); @@ -390,7 +439,7 @@ Eigen::Matrix Yessir::BodyToGlobal(euler_t angles, Eigen::Matrixstate.position.px = state.state_est_pos_x; this->state.position.py = state.state_est_pos_y; this->state.position.pz = state.state_est_pos_z; @@ -420,7 +469,7 @@ void Yessir::setState(KalmanState state) { * The Q matrix is the covariance matrix for the process noise and is * updated based on the time taken per cycle of the Kalman Filter Thread. */ -void Yessir::setQ(float dt, float sd) { +void EKF::setQ(float dt, float sd) { Q(0, 0) = pow(dt, 5) / 20; Q(0, 1) = pow(dt, 4) / 8; Q(0, 2) = pow(dt, 3) / 6; @@ -461,16 +510,28 @@ void Yessir::setQ(float dt, float sd) { * The F matrix is the state transition matrix and is defined * by how the states change over time. */ -void Yessir::setF(float dt) { - for (int i = 0; i < 3; i++) { - F_mat(3 * i, 3 * i + 1) = s_dt; - F_mat(3 * i, 3 * i + 2) = (dt * s_dt) / 2; - F_mat(3 * i + 1, 3 * i + 2) = s_dt; - - F_mat(3 * i, 3 * i) = 1; - F_mat(3 * i + 1, 3 * i + 1) = 1; - F_mat(3 * i + 2, 3 * i + 2) = 1; - } -} - -Yessir yessir; +void EKF::setF(float dt, float Ca, float Cn, float wx, float wy, float wz) { + Eigen::Matrix w = Eigen::Matrix::Zero(); + w(0, 0) = wx; + w(1, 0) = wy; + w(2, 0) = wz; + F_mat(0,1) = 1; + F_mat(3,4) = 1; + F_mat(6,7) = 1; + + F_mat(1,1) = -pi * Ca * r * r * rho * x_k(1,0) / m; + F_mat(1,4) = -pi * Ca * r * r * rho * x_k(4,0) / m + w(2,0); + F_mat(1,7) = -pi * Ca * r * r * rho * x_k(7,0) / m - w(1,0); + + F_mat(4,1) = pi * Cn * r * r * rho * x_k(1,0) / m - w(2,0); + F_mat(4,4) = pi * Cn * r * r * rho * x_k(2,0) / m + w(0,0); + F_mat(4,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + + F_mat(7,1) = pi * Cn * r * r * rho * x_k(1,0) / m + w(1,0); + F_mat(7,4) = pi * Cn * r * r * rho * x_k(2,0) / m - w(0,0); + F_mat(7,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + + float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); + float mach = velocity_magnitude / 343.0; + mach = std::round(mach / 0.04) * 0.04; + diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 70780ac8..a305e64c 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -8,29 +8,36 @@ #define NUM_SENSOR_INPUTS 4 #define ALTITUDE_BUFFER_SIZE 10 -class Yessir : public KalmanFilter +class EKF : public KalmanFilter { public: - Yessir(); + EKF(); void initialize(RocketSystems* args) override; - void priori() override; + void priori(float dt, Orientation &orientation, FSMState fsm); void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override; void setQ(float dt, float sd); - void setF(float dt); + void setF(float dt, float Ca, float Cn, float rho, float r, float m, Eigen::Matrix w); Eigen::Matrix BodyToGlobal(euler_t angles, Eigen::Matrix x_k); + Eigen::Matrix getThrust(float timestamp, Eigen::Matrix angles, FSMState FSM_state); KalmanData getState() override; void setState(KalmanState state) override; + void initializeAerodynamicData(); + float linearInterpolation(float x0, float y0, float x1, float y1, float x); + void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state); bool should_reinit = false; private: + std::map> aerodynamicData; float s_dt = 0.05f; float spectral_density_ = 13.0f; float kalman_apo = 0; KalmanState kalman_state; + FSMState last_fsm = FSMState::STATE_IDLE; + float stage_timestamp = 0; Eigen::Matrix init_accel = Eigen::Matrix::Zero(); Eigen::Matrix world_accel; @@ -38,4 +45,4 @@ class Yessir : public KalmanFilter KalmanData state; }; -extern Yessir yessir; +extern EKF ekf; diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/sensor_data.h index a7f09051..16669bf7 100644 --- a/MIDAS/src/sensor_data.h +++ b/MIDAS/src/sensor_data.h @@ -190,7 +190,13 @@ struct Orientation { return euler; } + Velocity orientation_velocity; + + Velocity getVelocity() const { + return orientation_velocity; + } + Acceleration orientation_acceleration; Acceleration linear_acceleration; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index c3e47ea2..a35aacc0 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -143,10 +143,6 @@ DECLARE_THREAD(buzzer, RocketSystems* arg) { } DECLARE_THREAD(kalman, RocketSystems* arg) { - // Orientation initial_orientation = arg->rocket_data.orientation.getRecent(); - // Barometer initial_barom_buf = arg->rocket_data.barometer.getRecent(); - // LowGData initial_accelerometer = arg->rocket_data.low_g.getRecent(); - //yessir.initialize(initial_orientation, initial_barom_buf, initial_accelerations); yessir.initialize(arg); TickType_t last = xTaskGetTickCount(); @@ -167,7 +163,7 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { .az = current_accelerometer.az }; float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; - + float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; yessir.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); KalmanData current_state = yessir.getState(); From 1bee699b00297dd528c1b8cefd8b47f46cac9cc5 Mon Sep 17 00:00:00 2001 From: Divij Date: Sat, 1 Feb 2025 21:52:18 -0600 Subject: [PATCH 04/22] added getThrust --- MIDAS/src/gnc/ekf.cpp | 57 +++++++++++++++++++++++-------------------- 1 file changed, 31 insertions(+), 26 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 40bc8882..8cbf9f58 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -265,37 +265,42 @@ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) Eigen::Matrix EKF::getThrust(float timestamp, Eigen::Matrix angles, FSMState FSM_state) { Eigen::Matrix thrust = Eigen::Matrix::Zero(); - if (FSM_state < FSMState::STATE_BURNOUT) { - // interpolate from O5500X_data - auto it = O5500X_data.lower_bound(time); - - if (it == moonburner_data.begin() || it == moonburner_data.end()) { - std::cout << "Timestep " << t << " is out of bounds for interpolation." << std::endl; - } else { - float x1 = it->first; - float y1 = it->second; - --it; - float x0 = it->first; - float y0 = it->second; - - float interpolatedValue = linearInterpolation(x0, y0, x1, y1, t); - std::cout << "Interpolated value at timestep " << t << " is " << interpolatedValue << std::endl; + if (FSM_state >= STATE_FIRST_BOOST){ + float interpolatedValue = 0; + if (FSM_state < FSMState::STATE_BURNOUT ) { + // interpolate from O5500X_data + if(timestamp >=0.009){ + auto it = O5500X_data.lower_bound(timestamp); + if (it != O5500X_data.end()) { + float x0 = it->first; + float y0 = it->second; + ++it; + float x1 = it->first; + float y1 = it->second; + interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); + } + } + } + else { + // interpolate from moonburner_data + if(timestamp >=0.083){ + auto it = moonburner_data.lower_bound(timestamp); + if (it != moonburner_data.end()) { + float x0 = it->first; + float y0 = it->second; + ++it; + float x1 = it->first; + float y1 = it->second; + interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); + } + } } - } - else { - // interpolate from moonburner_data - - } - thrust(0, 0) = 0; - thrust(1, 0) = 0; - thrust(2, 0) = 0; - } else if (FSM_state <= 9) { + thrust(0, 0) = thrust(0,0) * cos(angles(1,0)) * sin(angles(0,0)); thrust(1, 0) = thrust(1,0) * sin(angles(1,0)); thrust(2, 0) = thrust(2,0) * cos(angles(1,0)) * cos(angles(0,0)); - } - + } return thrust; } From eeacb1479731e18bee68677485ef1896d68149a8 Mon Sep 17 00:00:00 2001 From: Shishir Date: Sat, 1 Feb 2025 21:57:26 -0600 Subject: [PATCH 05/22] added the get thrust Co-authored-by: aneeshg5 Co-authored-by: divijgarg Co-authored-by: Keshav B --- MIDAS/src/gnc/ekf.cpp | 18 ++++++++++++++---- MIDAS/src/gnc/ekf.h | 8 ++++++-- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 8cbf9f58..928103a5 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -253,7 +253,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { 1.0; x_priori = (xdot * dt) + x_k; - EKF::set_F(dt, Ca, Cn, rho, w_x, w_y, w_z); + setF(dt, fsm, w_x, w_y, w_z); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } @@ -388,8 +388,8 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati stage_timestamp = 0; } stage_timestamp += dt; - setF(dt / 1000); - setQ(dt / 1000, sd); + setF(dt, FSM_state, orientation.roll, orientation.pitch, orientation.yaw); + setQ(dt, sd); priori(dt, orientation, FSM_state); update(barometer, acceleration, orientation, FSM_state); FSMState last_fsm = FSM_state; @@ -515,7 +515,7 @@ void EKF::setQ(float dt, float sd) { * The F matrix is the state transition matrix and is defined * by how the states change over time. */ -void EKF::setF(float dt, float Ca, float Cn, float wx, float wy, float wz) { +void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { Eigen::Matrix w = Eigen::Matrix::Zero(); w(0, 0) = wx; w(1, 0) = wy; @@ -524,6 +524,13 @@ void EKF::setF(float dt, float Ca, float Cn, float wx, float wy, float wz) { F_mat(3,4) = 1; F_mat(6,7) = 1; + float m = mass_sustainer; + float h = height_sustainer; + if (fsm < FSMState::STATE_BURNOUT) { + m = mass_full; + h= height_full; + } + F_mat(1,1) = -pi * Ca * r * r * rho * x_k(1,0) / m; F_mat(1,4) = -pi * Ca * r * r * rho * x_k(4,0) / m + w(2,0); F_mat(1,7) = -pi * Ca * r * r * rho * x_k(7,0) / m - w(1,0); @@ -539,4 +546,7 @@ void EKF::setF(float dt, float Ca, float Cn, float wx, float wy, float wz) { float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); float mach = velocity_magnitude / 343.0; mach = std::round(mach / 0.04) * 0.04; + std::tie(Ca, Cn, Cp) = aerodynamicData[mach]; + +} diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index a305e64c..70fab23c 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -3,6 +3,7 @@ #include "kalman_filter.h" #include "sensor_data.h" #include "Buffer.h" +#include #define NUM_STATES 9 #define NUM_SENSOR_INPUTS 4 @@ -17,7 +18,7 @@ class EKF : public KalmanFilter void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override; void setQ(float dt, float sd); - void setF(float dt, float Ca, float Cn, float rho, float r, float m, Eigen::Matrix w); + void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z); Eigen::Matrix BodyToGlobal(euler_t angles, Eigen::Matrix x_k); Eigen::Matrix getThrust(float timestamp, Eigen::Matrix angles, FSMState FSM_state); @@ -35,6 +36,9 @@ class EKF : public KalmanFilter float s_dt = 0.05f; float spectral_density_ = 13.0f; float kalman_apo = 0; + float Ca = 0; + float Cn = 0; + float Cp = 0; KalmanState kalman_state; FSMState last_fsm = FSMState::STATE_IDLE; float stage_timestamp = 0; @@ -45,4 +49,4 @@ class EKF : public KalmanFilter KalmanData state; }; -extern EKF ekf; +// extern EKF ekf; From ce35ec1c6a37acbc0a10087e33c53f18e015b17d Mon Sep 17 00:00:00 2001 From: Shishir Date: Sat, 1 Feb 2025 22:36:08 -0600 Subject: [PATCH 06/22] fixed compilation errors :) Co-authored-by: aneeshg5 Co-authored-by: divijgarg Co-authored-by: Keshav B --- MIDAS/src/gnc/ekf.cpp | 88 ++++++++++++++++------------------- MIDAS/src/gnc/ekf.h | 3 +- MIDAS/src/gnc/kalman_filter.h | 1 - 3 files changed, 42 insertions(+), 50 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 928103a5..b066beef 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -187,24 +187,9 @@ void EKF::initialize(RocketSystems* args) { } -/** - * @brief Estimates current state of the rocket without current sensor data - * - * The priori step of the Kalman filter is used to estimate the current state - * of the rocket without knowledge of the current sensor data. In other words, - * it extrapolates the state at time n+1 based on the state at time n. - */ - -Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { - Eigen::Matrix3f roll, pitch, yaw; - roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); - pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); - yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; - return yaw * pitch * roll * body_vect; -} void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { - Eigen::Matrix xdot = Eigen::Matrix::Zero(); + Eigen::Matrix xdot = Eigen::Matrix::Zero(); Velocity omega = orientation.getVelocity(); euler_t angles = orientation.getEuler(); @@ -232,13 +217,12 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { float Fay = 0; float Faz = 0; - Eigen::Matrix Fg_body = R.inverse() * gravity; + Eigen::Matrix Fg_body = GlobalToBody(angles, gravity); float Fgx = Fg_body(0,0); float Fgy = Fg_body(1,0); float Fgz = Fg_body(2,0); - Eigen::Matrix T = EKF::getThrust(stage_timestamp, ang_pos, fsm); + Eigen::Matrix T = EKF::getThrust(stage_timestamp, angles, fsm); float Ftx = T(0, 0); float Fty = T(1, 0); float Ftz = T(2, 0); - xdot << x_k(1,0), (Fax + Ftx + Fgx) / m - (w_y * x_k(7,0) - w_z * x_k(4,0)), @@ -263,10 +247,9 @@ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) } -Eigen::Matrix EKF::getThrust(float timestamp, Eigen::Matrix angles, FSMState FSM_state) { - Eigen::Matrix thrust = Eigen::Matrix::Zero(); +Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { + float interpolatedValue = 0; if (FSM_state >= STATE_FIRST_BOOST){ - float interpolatedValue = 0; if (FSM_state < FSMState::STATE_BURNOUT ) { // interpolate from O5500X_data if(timestamp >=0.009){ @@ -295,13 +278,10 @@ Eigen::Matrix EKF::getThrust(float timestamp, Eigen::Matrix interpolatedVector; + interpolatedVector(0,0) = interpolatedValue; + return BodyToGlobal(angles, interpolatedVector); } /** @@ -420,25 +400,6 @@ void EKF::initializeAerodynamicData() { } } - - - -/** - * @brief Converts a vector in the body frame to the global frame - * - * @param angles Roll, pitch, yaw angles - * @param body_vect Vector for rotation in the body frame - * @return Eigen::Matrix Rotated vector in the global frame - */ -Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { - Eigen::Matrix3f roll, pitch, yaw; - roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); - pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); - yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; - return yaw * pitch * roll * body_vect; -} - - /** * @brief Getter for state X * @@ -507,6 +468,37 @@ void EKF::setQ(float dt, float sd) { Q *= sd; } +/** + * @brief Converts a vector in the body frame to the global frame + * + * @param angles Roll, pitch, yaw angles + * @param body_vect Vector for rotation in the body frame + * @return Eigen::Matrix Rotated vector in the global frame + */ +Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { + Eigen::Matrix3f roll, pitch, yaw; + roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); + pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); + yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; + return yaw * pitch * roll * body_vect; +} + +Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { + //pysim code + // roll = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) + // pitch = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) + // yaw = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) + // return (yaw @ pitch @ roll).T @ world_vector + Eigen::Matrix3f roll; + roll << 1, 0, 0, 0, cos(angles.roll), -sin(angles.roll), 0, sin(angles.roll), cos(angles.roll); + Eigen::Matrix3f pitch; + pitch << cos(angles.pitch), 0, sin(angles.pitch), 0, 1, 0,-sin(angles.pitch), 0, cos(angles.pitch); + Eigen::Matrix3f yaw; + yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1; + Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; + return rotation_matrix.transpose() * world_vector; +} + /** * @brief Sets the F matrix given time step. * @@ -522,7 +514,7 @@ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { w(2, 0) = wz; F_mat(0,1) = 1; F_mat(3,4) = 1; - F_mat(6,7) = 1; + F_mat(6,7) = 1; float m = mass_sustainer; float h = height_sustainer; diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 70fab23c..fc71521c 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -20,7 +20,8 @@ class EKF : public KalmanFilter void setQ(float dt, float sd); void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z); Eigen::Matrix BodyToGlobal(euler_t angles, Eigen::Matrix x_k); - Eigen::Matrix getThrust(float timestamp, Eigen::Matrix angles, FSMState FSM_state); + Eigen::Matrix getThrust(float timestamp, euler_t angles, FSMState FSM_state); + Eigen::Matrix GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); KalmanData getState() override; void setState(KalmanState state) override; diff --git a/MIDAS/src/gnc/kalman_filter.h b/MIDAS/src/gnc/kalman_filter.h index cef1379e..61571599 100644 --- a/MIDAS/src/gnc/kalman_filter.h +++ b/MIDAS/src/gnc/kalman_filter.h @@ -59,7 +59,6 @@ class KalmanFilter virtual void initialize(RocketSystems* args) = 0; virtual void priori() = 0; virtual void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) = 0; - virtual KalmanData getState() = 0; virtual void setState(KalmanState state) = 0; }; From 1d19cbd8240e0b6610de39fef2caca651e52e373 Mon Sep 17 00:00:00 2001 From: Shishir Date: Sat, 1 Feb 2025 23:02:58 -0600 Subject: [PATCH 07/22] made small fixes to systems Co-authored-by: aneeshg5 Co-authored-by: divijgarg Co-authored-by: Keshav B --- MIDAS/src/gnc/ekf.cpp | 5 ++++- MIDAS/src/gnc/ekf.h | 5 +++-- MIDAS/src/systems.cpp | 16 ++++++++-------- 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index b066beef..fbce52e8 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -483,6 +483,8 @@ Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { //pysim code // roll = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) @@ -541,4 +543,5 @@ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { std::tie(Ca, Cn, Cp) = aerodynamicData[mach]; } - + +EKF ekf; diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index fc71521c..667851ec 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -13,7 +13,8 @@ class EKF : public KalmanFilter { public: EKF(); - void initialize(RocketSystems* args) override; + void initialize(RocketSystems* args) override; + void priori(); void priori(float dt, Orientation &orientation, FSMState fsm); void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override; @@ -50,4 +51,4 @@ class EKF : public KalmanFilter KalmanData state; }; -// extern EKF ekf; +extern EKF ekf; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index a35aacc0..aab1f993 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -1,7 +1,7 @@ #include "systems.h" #include "hal.h" -#include "gnc/yessir.h" +#include "gnc/ekf.h" #if defined(IS_SUSTAINER) && defined(IS_BOOSTER) #error "Only one of IS_SUSTAINER and IS_BOOSTER may be defined at the same time." @@ -143,14 +143,14 @@ DECLARE_THREAD(buzzer, RocketSystems* arg) { } DECLARE_THREAD(kalman, RocketSystems* arg) { - yessir.initialize(arg); + ekf.initialize(arg); TickType_t last = xTaskGetTickCount(); while (true) { - if(yessir.should_reinit){ - yessir.initialize(arg); + if(ekf.should_reinit){ + ekf.initialize(arg); TickType_t last = xTaskGetTickCount(); - yessir.should_reinit = false; + ekf.should_reinit = false; } // add the tick update function Barometer current_barom_buf = arg->rocket_data.barometer.getRecent(); @@ -164,8 +164,8 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { }; float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; - yessir.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); - KalmanData current_state = yessir.getState(); + ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); + KalmanData current_state = ekf.getState(); arg->rocket_data.kalman.update(current_state); @@ -187,7 +187,7 @@ DECLARE_THREAD(telemetry, RocketSystems* arg) { arg->tlm.acknowledgeReceived(); switch(command.command) { case CommandType::RESET_KF: - yessir.should_reinit = true; + ekf.should_reinit = true; break; default: break; From 1f1894ad944a8b3dd2a294a24d5c6715491a58bb Mon Sep 17 00:00:00 2001 From: Shishir Date: Sun, 2 Feb 2025 14:23:00 -0600 Subject: [PATCH 08/22] documentation --- MIDAS/src/gnc/ekf.cpp | 126 +++++++++++++++++++++++++++--------------- 1 file changed, 80 insertions(+), 46 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index fbce52e8..dfbdd3a8 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -6,33 +6,16 @@ EKF::EKF() : KalmanFilter() { initializeAerodynamicData(); } -/** - * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms - * apart - * - * The following for loop takes a series of barometer measurements on start - * up and takes the average of them in order to initialize the kalman filter - * to the correct initial barometric altitude. This is done so that the - * kalman filter takes minimal time to converge to an accurate state - * estimate. This process is significantly faster than allowing the state as - * letting the filter to converge to the correct state can take up to 3 min. - * This specific process was used because the barometric altitude will - * change depending on the weather and thus, the initial state estimate - * cannot be hard coded. A GPS altitude may be used instead but due to GPS - * losses during high speed/high altitude flight, it is inadvisable with the - * current hardware to use this as a solution. Reference frames should also - * be kept consistent (do not mix GPS altitude and barometric). - * - */ - +// constants const float pi = 3.14159268; -const float rho = 1.225; -const float r = 0.03935;//meters -const float height_full = 2.34; //height of rocket Full Stage - 2.34 m +const float rho = 1.225; // average air density +const float r = 0.03935; // meters +const float height_full = 2.34; // height of rocket Full Stage - 2.34 m const float height_sustainer = 1.34; -const float mass_full = 7.57; //kg Sustainer + Booster -const float mass_sustainer = 4.08; //kg Sustainer -// Moonburner motor +const float mass_full = 7.57; // kg Sustainer + Booster +const float mass_sustainer = 4.08; // kg Sustainer + +// Moonburner motor thrust curve (Sustainer) const std::map moonburner_data = { {0.083, 1333.469}, {0.13, 1368.376}, @@ -58,7 +41,7 @@ const std::map moonburner_data = { {11.994, 0.0} }; -// Booster motor +// O5500X motor thrust curve (Booster) std::map O5500X_data = { {0.009, 20.408}, {0.044, 7112.245}, @@ -102,6 +85,24 @@ std::map O5500X_data = { {3.997, 0.0} }; +/** + * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms + * apart + * + * The following for loop takes a series of barometer measurements on start + * up and takes the average of them in order to initialize the kalman filter + * to the correct initial barometric altitude. This is done so that the + * kalman filter takes minimal time to converge to an accurate state + * estimate. This process is significantly faster than allowing the state as + * letting the filter to converge to the correct state can take up to 3 min. + * This specific process was used because the barometric altitude will + * change depending on the weather and thus, the initial state estimate + * cannot be hard coded. A GPS altitude may be used instead but due to GPS + * losses during high speed/high altitude flight, it is inadvisable with the + * current hardware to use this as a solution. Reference frames should also + * be kept consistent (do not mix GPS altitude and barometric). + * + */ void EKF::initialize(RocketSystems* args) { Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); @@ -187,6 +188,13 @@ void EKF::initialize(RocketSystems* args) { } +/** + * @brief Estimates current state of the rocket without current sensor data + * + * The priori step of the Kalman filter is used to estimate the current state + * of the rocket without knowledge of the current sensor data. In other words, + * it extrapolates the state at time n+1 based on the state at time n. + */ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { Eigen::Matrix xdot = Eigen::Matrix::Zero(); @@ -197,15 +205,11 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { gravity(0, 0) = -9.81; Eigen::Matrix ang_pos = BodyToGlobal(angles, init_accel) + gravity; - float m = mass_sustainer; - float h = height_sustainer; + float m = mass_sustainer; float h = height_sustainer; if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; - h= height_full; + m = mass_full; h = height_full; } - float w_x = omega.vx; - float w_y = omega.vy; - float w_z = omega.vz; + float w_x = omega.vx; float w_y = omega.vy; float w_z = omega.vz; float J_x = 0.5 * m * r * r; float J_y = (1/3) * m * h * h + 0.25 * m * r * r; @@ -214,8 +218,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { float vel_mag_squared = x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0); float Fax = -0.5*rho*(vel_mag_squared)*float(Ca)*(pi*r*r); - float Fay = 0; - float Faz = 0; + float Fay = 0; float Faz = 0; Eigen::Matrix Fg_body = GlobalToBody(angles, gravity); float Fgx = Fg_body(0,0); float Fgy = Fg_body(1,0); float Fgz = Fg_body(2,0); @@ -229,7 +232,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { 1.0, x_k(4,0), - (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), // this might not be right + (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), 1.0, x_k(7,0), @@ -241,18 +244,33 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } - +/** + * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim + */ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) { return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); } - +/** + * @brief Returns the approximate thrust force from the motor given the thurst curve + * + * @param timestamp Time since most recent ignition + * @param angles Current orientation of the rocket + * @param FSM_state Current FSM state + * + * @return Thrust force in the body frame + * + * The thrust force is calculated by interpolating the thrust curve data which is stored in an ordered map (see top of file). + * The thrust curve data is different for the booster and sustainer stages, so the function checks the FSM state to determine + * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. + * The thrust force is then rotated into the body frame using the BodyToGlobal function. + */ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { float interpolatedValue = 0; if (FSM_state >= STATE_FIRST_BOOST){ if (FSM_state < FSMState::STATE_BURNOUT ) { - // interpolate from O5500X_data - if(timestamp >=0.009){ + // first stage + if(timestamp >= 0.009){ auto it = O5500X_data.lower_bound(timestamp); if (it != O5500X_data.end()) { float x0 = it->first; @@ -265,8 +283,8 @@ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMSt } } else { - // interpolate from moonburner_data - if(timestamp >=0.083){ + if (timestamp >= 0.083){ + // second stage auto it = moonburner_data.lower_bound(timestamp); if (it != moonburner_data.end()) { float x0 = it->first; @@ -334,8 +352,6 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); P_k = (identity - K * H) * P_priori; - // Joseph (Expanded) Form - // P_k = (identity - K * H) * P_priori * (identity - K * H).transpose() + K * R * K.transpose(); kalman_state.state_est_pos_x = x_k(0, 0); kalman_state.state_est_vel_x = x_k(1, 0); @@ -375,8 +391,13 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati FSMState last_fsm = FSM_state; } } -//LookUpMap +/** + * @brief Initializes the aerodynamic data from a CSV file + * + * The function reads a CSV file containing aerodynamic data and stores it in a map. + * The key is the Mach number, and the value is a tuple containing aerodynamic coefficients. + */ void EKF::initializeAerodynamicData() { std::ifstream file("LookUp.csv"); std::string line; @@ -483,8 +504,20 @@ Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix Rotated vector in the body frame + * + */ Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { //pysim code // roll = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) @@ -507,7 +540,8 @@ Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix * @param dt Time step calculated by the Kalman Filter Thread * * The F matrix is the state transition matrix and is defined - * by how the states change over time. + * by how the states change over time and also depends on the + * current state of the rocket. */ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { Eigen::Matrix w = Eigen::Matrix::Zero(); From e966edefb7c04984485e57a94bb843dcfbdb43fd Mon Sep 17 00:00:00 2001 From: Keshav B Date: Mon, 3 Feb 2025 19:50:40 -0600 Subject: [PATCH 09/22] Fixed memory allocation on ekf.cpp Co-authored-by: divijgarg --- MIDAS/src/gnc/ekf.cpp | 157 ++++++++++++++++++++++++++++-------------- MIDAS/src/gnc/ekf.h | 8 +-- 2 files changed, 111 insertions(+), 54 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index dfbdd3a8..8c66a595 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,20 +1,23 @@ #include "ekf.h" #include "finite-state-machines/fsm_states.h" + EKF::EKF() : KalmanFilter() { state = KalmanData(); initializeAerodynamicData(); } + // constants const float pi = 3.14159268; const float rho = 1.225; // average air density const float r = 0.03935; // meters -const float height_full = 2.34; // height of rocket Full Stage - 2.34 m +const float height_full = 2.34; // height of rocket Full Stage - 2.34 m const float height_sustainer = 1.34; const float mass_full = 7.57; // kg Sustainer + Booster const float mass_sustainer = 4.08; // kg Sustainer + // Moonburner motor thrust curve (Sustainer) const std::map moonburner_data = { {0.083, 1333.469}, @@ -41,6 +44,7 @@ const std::map moonburner_data = { {11.994, 0.0} }; + // O5500X motor thrust curve (Booster) std::map O5500X_data = { {0.009, 20.408}, @@ -85,6 +89,7 @@ std::map O5500X_data = { {3.997, 0.0} }; + /** * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms * apart @@ -104,10 +109,11 @@ std::map O5500X_data = { * */ + void EKF::initialize(RocketSystems* args) { Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); float sum = 0; - + for (int i = 0; i < 30; i++) { Barometer barometer = args->rocket_data.barometer.getRecent(); LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); @@ -118,27 +124,31 @@ void EKF::initialize(RocketSystems* args) { }; sum += barometer.altitude; + init_accel(0, 0) += accelerations.az; init_accel(1, 0) += accelerations.ay; init_accel(2, 0) += -accelerations.ax; THREAD_SLEEP(100); } + init_accel(0, 0) /= 30; init_accel(1, 0) /= 30; init_accel(2, 0) /= 30; + euler_t euler = orientation.getEuler(); euler.yaw = -euler.yaw; - world_accel = BodyToGlobal(euler, init_accel); // set x_k x_k(0, 0) = sum / 30; x_k(3, 0) = 0; x_k(6, 0) = 0; + F_mat.setZero(); // Initialize with zeros + Q(0, 0) = pow(s_dt, 5) / 20; Q(0, 1) = pow(s_dt, 4) / 8; Q(0, 2) = pow(s_dt, 3) / 6; @@ -149,6 +159,7 @@ void EKF::initialize(RocketSystems* args) { Q(2, 0) = Q(0, 2); Q(2, 1) = Q(1, 2); + Q(3, 3) = pow(s_dt, 5) / 20; Q(3, 4) = pow(s_dt, 4) / 8; Q(3, 5) = pow(s_dt, 3) / 6; @@ -159,6 +170,7 @@ void EKF::initialize(RocketSystems* args) { Q(5, 3) = Q(3, 5); Q(5, 4) = Q(4, 5); + Q(6, 6) = pow(s_dt, 5) / 20; Q(6, 7) = pow(s_dt, 4) / 8; Q(6, 8) = pow(s_dt, 3) / 6; @@ -169,25 +181,31 @@ void EKF::initialize(RocketSystems* args) { Q(8, 6) = Q(6, 8); Q(8, 7) = Q(7, 8); + // set H H(0, 0) = 1; H(1, 2) = 1; H(2, 5) = 1; H(3, 8) = 1; + Q = Q * spectral_density_; + // set R R(0, 0) = 2.0; R(1, 1) = 1.9; R(2, 2) = 10; R(3, 3) = 10; + // set B (don't care about what's in B since we have no control input) B(2, 0) = -1; + } + /** * @brief Estimates current state of the rocket without current sensor data * @@ -196,54 +214,62 @@ void EKF::initialize(RocketSystems* args) { * it extrapolates the state at time n+1 based on the state at time n. */ + void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { Eigen::Matrix xdot = Eigen::Matrix::Zero(); - Velocity omega = orientation.getVelocity(); euler_t angles = orientation.getEuler(); - Eigen::Matrix gravity = Eigen::Matrix::Zero(); - gravity(0, 0) = -9.81; - Eigen::Matrix ang_pos = BodyToGlobal(angles, init_accel) + gravity; - + // Eigen::Matrix gravity = Eigen::Matrix::Zero(); + gravity(0, 0) = -9.81; float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) { + if (fsm < FSMState::STATE_BURNOUT) { m = mass_full; h = height_full; } float w_x = omega.vx; float w_y = omega.vy; float w_z = omega.vz; - + float J_x = 0.5 * m * r * r; float J_y = (1/3) * m * h * h + 0.25 * m * r * r; float J_z = J_y; float vel_mag_squared = x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0); + float Fax = -0.5*rho*(vel_mag_squared)*float(Ca)*(pi*r*r); float Fay = 0; float Faz = 0; - Eigen::Matrix Fg_body = GlobalToBody(angles, gravity); - float Fgx = Fg_body(0,0); float Fgy = Fg_body(1,0); float Fgz = Fg_body(2,0); - Eigen::Matrix T = EKF::getThrust(stage_timestamp, angles, fsm); - float Ftx = T(0, 0); float Fty = T(1, 0); float Ftz = T(2, 0); + Eigen::Matrix *Fg_body = (GlobalToBody(angles, gravity)); + float Fgx = (*Fg_body)(0,0); float Fgy = (*Fg_body)(1,0); float Fgz = (*Fg_body)(2,0); - + delete Fg_body; + + Eigen::Matrix * T = EKF::getThrust(stage_timestamp, angles, fsm); + float Ftx = (*T)(0, 0); float Fty = (*T)(1, 0); float Ftz = (*T)(2, 0); + + delete T; + + xdot << x_k(1,0), (Fax + Ftx + Fgx) / m - (w_y * x_k(7,0) - w_z * x_k(4,0)), 1.0, + x_k(4,0), (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), 1.0, + x_k(7,0), (Faz + Ftz + Fgz) / m - (w_x * x_k(4,0) - w_y * x_k(1,0)), 1.0; + x_priori = (xdot * dt) + x_k; setF(dt, fsm, w_x, w_y, w_z); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } + /** * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim */ @@ -251,21 +277,22 @@ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); } + /** * @brief Returns the approximate thrust force from the motor given the thurst curve - * + * * @param timestamp Time since most recent ignition * @param angles Current orientation of the rocket * @param FSM_state Current FSM state - * + * * @return Thrust force in the body frame - * + * * The thrust force is calculated by interpolating the thrust curve data which is stored in an ordered map (see top of file). - * The thrust curve data is different for the booster and sustainer stages, so the function checks the FSM state to determine - * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. + * The thrust curve data is different for the booster and sustainer stages, so the function checks the FSM state to determine + * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. * The thrust force is then rotated into the body frame using the BodyToGlobal function. */ -Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { +Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { float interpolatedValue = 0; if (FSM_state >= STATE_FIRST_BOOST){ if (FSM_state < FSMState::STATE_BURNOUT ) { @@ -278,10 +305,10 @@ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMSt ++it; float x1 = it->first; float y1 = it->second; - interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); + interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); } } - } + } else { if (timestamp >= 0.083){ // second stage @@ -302,6 +329,7 @@ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMSt return BodyToGlobal(angles, interpolatedVector); } + /** * @brief Update Kalman Gain and state estimate with current sensor data * @@ -311,7 +339,7 @@ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMSt * */ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { - if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { + if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { float sum = 0; float data[10]; alt_buffer.readSlice(data, 0, 10); @@ -324,35 +352,44 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori H(1, 2) = 0; } + Eigen::Matrix S_k = Eigen::Matrix::Zero(); S_k = (((H * P_priori * H.transpose()) + R)).inverse(); Eigen::Matrix identity = Eigen::Matrix::Identity(); K = (P_priori * H.transpose()) * S_k; + // Sensor Measurements Eigen::Matrix accel = Eigen::Matrix::Zero(); - + accel(0, 0) = acceleration.az - 0.045; accel(1, 0) = acceleration.ay - 0.065; accel(2, 0) = -acceleration.ax - 0.06; + euler_t angles = orientation.getEuler(); angles.yaw = -angles.yaw; - Eigen::Matrix acc = BodyToGlobal(angles, accel); - y_k(1, 0) = (acc(0)) * 9.81 - 9.81; - y_k(2, 0) = (acc(1)) * 9.81; - y_k(3, 0) = (acc(2)) * 9.81; + Eigen::Matrix * acc = (BodyToGlobal(angles, accel)); + + + y_k(1, 0) = ((*acc)(0)) * 9.81 - 9.81; + y_k(2, 0) = ((*acc)(1)) * 9.81; + y_k(3, 0) = ((*acc)(2)) * 9.81; + + delete acc; y_k(0, 0) = barometer.altitude; alt_buffer.push(barometer.altitude); + + // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); P_k = (identity - K * H) * P_priori; - + kalman_state.state_est_pos_x = x_k(0, 0); kalman_state.state_est_vel_x = x_k(1, 0); kalman_state.state_est_accel_x = x_k(2, 0); @@ -363,11 +400,13 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori kalman_state.state_est_vel_z = x_k(7, 0); kalman_state.state_est_accel_z = x_k(8, 0); + state.position = (Position){kalman_state.state_est_pos_x,kalman_state.state_est_pos_y,kalman_state.state_est_pos_z}; state.velocity = (Velocity){kalman_state.state_est_vel_x,kalman_state.state_est_vel_y,kalman_state.state_est_vel_z}; state.acceleration = (Acceleration){kalman_state.state_est_accel_x,kalman_state.state_est_accel_y,kalman_state.state_est_accel_z}; } + /** * @brief Run Kalman filter calculations as long as FSM has passed IDLE * @@ -392,6 +431,7 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati } } + /** * @brief Initializes the aerodynamic data from a CSV file * @@ -401,19 +441,19 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati void EKF::initializeAerodynamicData() { std::ifstream file("LookUp.csv"); std::string line; - + // Skip the header line std::getline(file, line); - + while (std::getline(file, line)) { std::istringstream iss(line); std::string token; std::vector row; - + while (std::getline(iss, token, ',')) { row.push_back(std::stod(token)); } - + if (row.size() >= 5) { double mach = std::round(row[0] / 0.04) * 0.04; aerodynamicData[mach] = std::make_tuple(row[2], row[3], row[4]); @@ -421,6 +461,7 @@ void EKF::initializeAerodynamicData() { } } + /** * @brief Getter for state X * @@ -430,6 +471,7 @@ KalmanData EKF::getState() { return state; } + /** * @brief Sets state vector x * @@ -447,6 +489,7 @@ void EKF::setState(KalmanState state) { this->state.velocity.vz = state.state_est_vel_z; } + /** * @brief Sets the Q matrix given time step and spectral density. * @@ -476,6 +519,7 @@ void EKF::setQ(float dt, float sd) { Q(5, 3) = Q(3, 5); Q(5, 4) = Q(4, 5); + Q(6, 6) = pow(dt, 5) / 20; Q(6, 7) = pow(dt, 4) / 8; Q(6, 8) = pow(dt, 3) / 6; @@ -486,9 +530,11 @@ void EKF::setQ(float dt, float sd) { Q(8, 6) = Q(6, 8); Q(8, 7) = Q(7, 8); + Q *= sd; } + /** * @brief Converts a vector in the body frame to the global frame * @@ -496,34 +542,34 @@ void EKF::setQ(float dt, float sd) { * @param body_vect Vector for rotation in the body frame * @return Eigen::Matrix Rotated vector in the global frame */ -Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { +Eigen::Matrix * EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { Eigen::Matrix3f roll, pitch, yaw; roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; - return yaw * pitch * roll * body_vect; + + Eigen::Matrix * global = new Eigen::Matrix(yaw * pitch * roll * body_vect); + return global; } + /** * THIS IS A PLACEHOLDER FUNCTION SO WE CAN ABSTRACT FROM `kalman_filter.h` */ void EKF::priori(){}; + /** * @brief Converts a vector in the global frame to the body frame - * + * * @param angles Roll, pitch, yaw angles * @param world_vector Vector for rotation in the global frame - * + * * @return Eigen::Matrix Rotated vector in the body frame - * + * */ -Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { - //pysim code - // roll = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) - // pitch = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) - // yaw = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) - // return (yaw @ pitch @ roll).T @ world_vector +Eigen::Matrix * EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { + Eigen::Matrix3f roll; roll << 1, 0, 0, 0, cos(angles.roll), -sin(angles.roll), 0, sin(angles.roll), cos(angles.roll); Eigen::Matrix3f pitch; @@ -531,7 +577,10 @@ Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix Eigen::Matrix3f yaw; yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1; Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; - return rotation_matrix.transpose() * world_vector; + (rotation_matrix).transpose(); + Eigen::Matrix * to_return = new Eigen::Matrix(rotation_matrix*world_vector); + return to_return; + } /** @@ -550,32 +599,40 @@ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { w(2, 0) = wz; F_mat(0,1) = 1; F_mat(3,4) = 1; - F_mat(6,7) = 1; + F_mat(6,7) = 1; + float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; + if (fsm < FSMState::STATE_BURNOUT) { + m = mass_full; h= height_full; } + F_mat(1,1) = -pi * Ca * r * r * rho * x_k(1,0) / m; F_mat(1,4) = -pi * Ca * r * r * rho * x_k(4,0) / m + w(2,0); F_mat(1,7) = -pi * Ca * r * r * rho * x_k(7,0) / m - w(1,0); + F_mat(4,1) = pi * Cn * r * r * rho * x_k(1,0) / m - w(2,0); F_mat(4,4) = pi * Cn * r * r * rho * x_k(2,0) / m + w(0,0); F_mat(4,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + F_mat(7,1) = pi * Cn * r * r * rho * x_k(1,0) / m + w(1,0); F_mat(7,4) = pi * Cn * r * r * rho * x_k(2,0) / m - w(0,0); F_mat(7,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); float mach = velocity_magnitude / 343.0; mach = std::round(mach / 0.04) * 0.04; std::tie(Ca, Cn, Cp) = aerodynamicData[mach]; - + } EKF ekf; + + + diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 667851ec..f874d6e2 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -20,9 +20,9 @@ class EKF : public KalmanFilter void setQ(float dt, float sd); void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z); - Eigen::Matrix BodyToGlobal(euler_t angles, Eigen::Matrix x_k); - Eigen::Matrix getThrust(float timestamp, euler_t angles, FSMState FSM_state); - Eigen::Matrix GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); + Eigen::Matrix *BodyToGlobal(euler_t angles, Eigen::Matrix x_k); + Eigen::Matrix *getThrust(float timestamp, euler_t angles, FSMState FSM_state); + Eigen::Matrix *GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); KalmanData getState() override; void setState(KalmanState state) override; @@ -41,12 +41,12 @@ class EKF : public KalmanFilter float Ca = 0; float Cn = 0; float Cp = 0; + Eigen::Matrix gravity = Eigen::Matrix::Zero(); KalmanState kalman_state; FSMState last_fsm = FSMState::STATE_IDLE; float stage_timestamp = 0; Eigen::Matrix init_accel = Eigen::Matrix::Zero(); - Eigen::Matrix world_accel; Buffer alt_buffer; KalmanData state; }; From 5ece4a06f6f425e56e9081b5f5dc0d1103e6299b Mon Sep 17 00:00:00 2001 From: Shishir Date: Sat, 8 Feb 2025 13:53:55 -0600 Subject: [PATCH 10/22] made changes based on Zyun and Rbhog's comments, changed the lookup to an array of structs --- MIDAS/src/gnc/LookUp.csv | 26 ----------- MIDAS/src/gnc/ekf.cpp | 99 +++++++++++++++++++++++----------------- MIDAS/src/gnc/ekf.h | 5 +- 3 files changed, 57 insertions(+), 73 deletions(-) delete mode 100644 MIDAS/src/gnc/LookUp.csv diff --git a/MIDAS/src/gnc/LookUp.csv b/MIDAS/src/gnc/LookUp.csv deleted file mode 100644 index cfd5768a..00000000 --- a/MIDAS/src/gnc/LookUp.csv +++ /dev/null @@ -1,26 +0,0 @@ -Mach,Alpha,CA Power-On,CN,CP -0.04,4,1.332142905,1.1827808455016526632575949416,60.826787196815171703275113218 -0.08,4,1.326587387,1.1827808455016526632575949416,60.826787196815171703275113218 -0.12,4,1.31558627,1.1827808455016526632575949416,60.826787196815171703275113218 -0.16,4,1.306063953,1.1827808455016526632575949416,60.826787196815171703275113218 -0.20,4,1.298117084,1.1827808455016526632575949416,60.826787196815171703275113218 -0.24,4,1.291411025,1.1827808455016526632575949416,60.826787196815171703275113218 -0.28,4,1.290279857,1.1827808455016526632575949416,60.826787196815171703275113218 -0.32,4,1.291431043,1.1827808455016526632575949416,60.826787196815171703275113218 -0.36,4,1.293170653,1.1827808455016526632575949416,60.826787196815171703275113218 -0.40,4,1.295385827,1.1827808455016526632575949416,60.826787196815171703275113218 -0.44,4,1.297991738,1.1827808455016526632575949416,60.826787196815171703275113218 -0.48,4,1.300924032,1.1827808455016526632575949416,60.826787196815171703275113218 -0.52,4,1.304132086,1.1827808455016526632575949416,60.826787196815171703275113218 -0.56,4,1.309039395,1.1827808455016526632575949416,60.826787196815171703275113218 -0.60,4,1.314605487,1.1827808455016526632575949416,60.826787196815171703275113218 -0.64,4,1.330699437,1.1827808455016526632575949416,60.826787196815171703275113218 -0.68,4,1.346695167,1.1827808455016526632575949416,60.826787196815171703275113218 -0.72,4,1.362693183,1.1827808455016526632575949416,60.826787196815171703275113218 -0.76,4,1.378693074,1.1827808455016526632575949416,60.826787196815171703275113218 -0.80,4,1.394695194,1.1827808455016526632575949416,60.826787196815171703275113218 -0.84,4,1.41069913,1.1827808455016526632575949416,60.826787196815171703275113218 -0.88,4,1.426705046,1.1827808455016526632575949416,60.826787196815171703275113218 -0.92,4,1.473732816,1.218959468,60.91848997 -0.96,4,1.582395672,1.291316713,61.10189551 -1.00,4,1.681494886,1.363673958,61.28530105 diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 8c66a595..2f2bc12f 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -4,18 +4,59 @@ EKF::EKF() : KalmanFilter() { state = KalmanData(); - initializeAerodynamicData(); } // constants const float pi = 3.14159268; +const float a = 343.0; // (m/s) speed of sound const float rho = 1.225; // average air density -const float r = 0.03935; // meters -const float height_full = 2.34; // height of rocket Full Stage - 2.34 m -const float height_sustainer = 1.34; -const float mass_full = 7.57; // kg Sustainer + Booster -const float mass_sustainer = 4.08; // kg Sustainer +const float r = 0.03935; // (m) +const float height_full = 2.34; // (m) height of rocket Full Stage +const float height_sustainer = 1.34; // (m) height of rocket Sustainer +const float mass_full = 7.57; // (kg) Sustainer + Booster +const float mass_sustainer = 4.08; // (kg) Sustainer + +typedef struct { + float mach; + float alpha; + float CA_power_on; + float CN; + float CP; +} AeroCoeff; + +// stores the aerodynamic coefficients for the corresponding Mach number +const AeroCoeff aero_data[] = { + {0.04, 4, 1.332142905, 1.1827808455, 60.8267871968}, + {0.08, 4, 1.326587387, 1.1827808455, 60.8267871968}, + {0.12, 4, 1.31558627, 1.1827808455, 60.8267871968}, + {0.16, 4, 1.306063953, 1.1827808455, 60.8267871968}, + {0.20, 4, 1.298117084, 1.1827808455, 60.8267871968}, + {0.24, 4, 1.291411025, 1.1827808455, 60.8267871968}, + {0.28, 4, 1.290279857, 1.1827808455, 60.8267871968}, + {0.32, 4, 1.291431043, 1.1827808455, 60.8267871968}, + {0.36, 4, 1.293170653, 1.1827808455, 60.8267871968}, + {0.40, 4, 1.295385827, 1.1827808455, 60.8267871968}, + {0.44, 4, 1.297991738, 1.1827808455, 60.8267871968}, + {0.48, 4, 1.300924032, 1.1827808455, 60.8267871968}, + {0.52, 4, 1.304132086, 1.1827808455, 60.8267871968}, + {0.56, 4, 1.309039395, 1.1827808455, 60.8267871968}, + {0.60, 4, 1.314605487, 1.1827808455, 60.8267871968}, + {0.64, 4, 1.330699437, 1.1827808455, 60.8267871968}, + {0.68, 4, 1.346695167, 1.1827808455, 60.8267871968}, + {0.72, 4, 1.362693183, 1.1827808455, 60.8267871968}, + {0.76, 4, 1.378693074, 1.1827808455, 60.8267871968}, + {0.80, 4, 1.394695194, 1.1827808455, 60.8267871968}, + {0.84, 4, 1.41069913, 1.1827808455, 60.8267871968}, + {0.88, 4, 1.426705046, 1.1827808455, 60.8267871968}, + {0.92, 4, 1.473732816, 1.218959468, 60.91848997}, + {0.96, 4, 1.582395672, 1.291316713, 61.10189551}, + {1.00, 4, 1.681494886, 1.363673958, 61.28530105}, +}; + +// Number of entries +#define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0])) + // Moonburner motor thrust curve (Sustainer) @@ -432,36 +473,6 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati } -/** - * @brief Initializes the aerodynamic data from a CSV file - * - * The function reads a CSV file containing aerodynamic data and stores it in a map. - * The key is the Mach number, and the value is a tuple containing aerodynamic coefficients. - */ -void EKF::initializeAerodynamicData() { - std::ifstream file("LookUp.csv"); - std::string line; - - // Skip the header line - std::getline(file, line); - - while (std::getline(file, line)) { - std::istringstream iss(line); - std::string token; - std::vector row; - - while (std::getline(iss, token, ',')) { - row.push_back(std::stod(token)); - } - - if (row.size() >= 5) { - double mach = std::round(row[0] / 0.04) * 0.04; - aerodynamicData[mach] = std::make_tuple(row[2], row[3], row[4]); - } - } -} - - /** * @brief Getter for state X * @@ -601,6 +612,15 @@ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { F_mat(3,4) = 1; F_mat(6,7) = 1; + + float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); + float mach = velocity_magnitude / a; + int index = std::round(mach / 0.04); + index = std::clamp(index, 0, (int)AERO_DATA_SIZE - 1); + + Ca = aero_data[index].CA_power_on; + Cn = aero_data[index].CN; + Cp = aero_data[index].CP; float m = mass_sustainer; float h = height_sustainer; @@ -623,13 +643,6 @@ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { F_mat(7,1) = pi * Cn * r * r * rho * x_k(1,0) / m + w(1,0); F_mat(7,4) = pi * Cn * r * r * rho * x_k(2,0) / m - w(0,0); F_mat(7,7) = pi * Cn * r * r * rho * x_k(3,0) / m; - - - float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); - float mach = velocity_magnitude / 343.0; - mach = std::round(mach / 0.04) * 0.04; - std::tie(Ca, Cn, Cp) = aerodynamicData[mach]; - } EKF ekf; diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index f874d6e2..2c8f609e 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -3,7 +3,6 @@ #include "kalman_filter.h" #include "sensor_data.h" #include "Buffer.h" -#include #define NUM_STATES 9 #define NUM_SENSOR_INPUTS 4 @@ -20,21 +19,19 @@ class EKF : public KalmanFilter void setQ(float dt, float sd); void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z); - Eigen::Matrix *BodyToGlobal(euler_t angles, Eigen::Matrix x_k); Eigen::Matrix *getThrust(float timestamp, euler_t angles, FSMState FSM_state); + Eigen::Matrix *BodyToGlobal(euler_t angles, Eigen::Matrix x_k); Eigen::Matrix *GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); KalmanData getState() override; void setState(KalmanState state) override; - void initializeAerodynamicData(); float linearInterpolation(float x0, float y0, float x1, float y1, float x); void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state); bool should_reinit = false; private: - std::map> aerodynamicData; float s_dt = 0.05f; float spectral_density_ = 13.0f; float kalman_apo = 0; From d48479c170b4ad13e28c9eab58b6edb762e0a3c9 Mon Sep 17 00:00:00 2001 From: Divij Date: Mon, 10 Feb 2025 18:31:11 -0600 Subject: [PATCH 11/22] memory changes --- MIDAS/src/gnc/ekf.cpp | 306 ++++++++++++++++++++---------------------- MIDAS/src/gnc/ekf.h | 4 +- 2 files changed, 145 insertions(+), 165 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 2f2bc12f..c5577356 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,23 +1,23 @@ #include "ekf.h" #include "finite-state-machines/fsm_states.h" - -EKF::EKF() : KalmanFilter() { +EKF::EKF() : KalmanFilter() +{ state = KalmanData(); } - // constants const float pi = 3.14159268; -const float a = 343.0; // (m/s) speed of sound -const float rho = 1.225; // average air density -const float r = 0.03935; // (m) -const float height_full = 2.34; // (m) height of rocket Full Stage -const float height_sustainer = 1.34; // (m) height of rocket Sustainer -const float mass_full = 7.57; // (kg) Sustainer + Booster -const float mass_sustainer = 4.08; // (kg) Sustainer - -typedef struct { +const float a = 343.0; // (m/s) speed of sound +const float rho = 1.225; // average air density +const float r = 0.03935; // (m) +const float height_full = 2.34; // (m) height of rocket Full Stage +const float height_sustainer = 1.34; // (m) height of rocket Sustainer +const float mass_full = 7.57; // (kg) Sustainer + Booster +const float mass_sustainer = 4.08; // (kg) Sustainer + +typedef struct +{ float mach; float alpha; float CA_power_on; @@ -29,7 +29,7 @@ typedef struct { const AeroCoeff aero_data[] = { {0.04, 4, 1.332142905, 1.1827808455, 60.8267871968}, {0.08, 4, 1.326587387, 1.1827808455, 60.8267871968}, - {0.12, 4, 1.31558627, 1.1827808455, 60.8267871968}, + {0.12, 4, 1.31558627, 1.1827808455, 60.8267871968}, {0.16, 4, 1.306063953, 1.1827808455, 60.8267871968}, {0.20, 4, 1.298117084, 1.1827808455, 60.8267871968}, {0.24, 4, 1.291411025, 1.1827808455, 60.8267871968}, @@ -47,18 +47,16 @@ const AeroCoeff aero_data[] = { {0.72, 4, 1.362693183, 1.1827808455, 60.8267871968}, {0.76, 4, 1.378693074, 1.1827808455, 60.8267871968}, {0.80, 4, 1.394695194, 1.1827808455, 60.8267871968}, - {0.84, 4, 1.41069913, 1.1827808455, 60.8267871968}, + {0.84, 4, 1.41069913, 1.1827808455, 60.8267871968}, {0.88, 4, 1.426705046, 1.1827808455, 60.8267871968}, - {0.92, 4, 1.473732816, 1.218959468, 60.91848997}, - {0.96, 4, 1.582395672, 1.291316713, 61.10189551}, - {1.00, 4, 1.681494886, 1.363673958, 61.28530105}, + {0.92, 4, 1.473732816, 1.218959468, 60.91848997}, + {0.96, 4, 1.582395672, 1.291316713, 61.10189551}, + {1.00, 4, 1.681494886, 1.363673958, 61.28530105}, }; // Number of entries #define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0])) - - // Moonburner motor thrust curve (Sustainer) const std::map moonburner_data = { {0.083, 1333.469}, @@ -82,9 +80,7 @@ const std::map moonburner_data = { {9.993, 207.118}, {10.514, 137.303}, {11.496, 34.908}, - {11.994, 0.0} -}; - + {11.994, 0.0}}; // O5500X motor thrust curve (Booster) std::map O5500X_data = { @@ -127,9 +123,7 @@ std::map O5500X_data = { {3.853, 489.796}, {3.897, 285.714}, {3.981, 20.408}, - {3.997, 0.0} -}; - + {3.997, 0.0}}; /** * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms @@ -150,34 +144,31 @@ std::map O5500X_data = { * */ - -void EKF::initialize(RocketSystems* args) { +void EKF::initialize(RocketSystems *args) +{ Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); float sum = 0; - - for (int i = 0; i < 30; i++) { + + for (int i = 0; i < 30; i++) + { Barometer barometer = args->rocket_data.barometer.getRecent(); LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); Acceleration accelerations = { .ax = initial_accelerometer.ax, .ay = initial_accelerometer.ay, - .az = initial_accelerometer.az - }; + .az = initial_accelerometer.az}; sum += barometer.altitude; - init_accel(0, 0) += accelerations.az; init_accel(1, 0) += accelerations.ay; init_accel(2, 0) += -accelerations.ax; THREAD_SLEEP(100); } - init_accel(0, 0) /= 30; init_accel(1, 0) /= 30; init_accel(2, 0) /= 30; - euler_t euler = orientation.getEuler(); euler.yaw = -euler.yaw; @@ -186,9 +177,7 @@ void EKF::initialize(RocketSystems* args) { x_k(3, 0) = 0; x_k(6, 0) = 0; - - F_mat.setZero(); // Initialize with zeros - + F_mat.setZero(); // Initialize with zeros Q(0, 0) = pow(s_dt, 5) / 20; Q(0, 1) = pow(s_dt, 4) / 8; @@ -200,7 +189,6 @@ void EKF::initialize(RocketSystems* args) { Q(2, 0) = Q(0, 2); Q(2, 1) = Q(1, 2); - Q(3, 3) = pow(s_dt, 5) / 20; Q(3, 4) = pow(s_dt, 4) / 8; Q(3, 5) = pow(s_dt, 3) / 6; @@ -211,7 +199,6 @@ void EKF::initialize(RocketSystems* args) { Q(5, 3) = Q(3, 5); Q(5, 4) = Q(4, 5); - Q(6, 6) = pow(s_dt, 5) / 20; Q(6, 7) = pow(s_dt, 4) / 8; Q(6, 8) = pow(s_dt, 3) / 6; @@ -222,31 +209,24 @@ void EKF::initialize(RocketSystems* args) { Q(8, 6) = Q(6, 8); Q(8, 7) = Q(7, 8); - // set H H(0, 0) = 1; H(1, 2) = 1; H(2, 5) = 1; H(3, 8) = 1; - Q = Q * spectral_density_; - // set R R(0, 0) = 2.0; R(1, 1) = 1.9; R(2, 2) = 10; R(3, 3) = 10; - // set B (don't care about what's in B since we have no control input) B(2, 0) = -1; - - } - /** * @brief Estimates current state of the rocket without current sensor data * @@ -255,70 +235,73 @@ void EKF::initialize(RocketSystems* args) { * it extrapolates the state at time n+1 based on the state at time n. */ - -void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { +void EKF::priori(float dt, Orientation &orientation, FSMState fsm) +{ Eigen::Matrix xdot = Eigen::Matrix::Zero(); Velocity omega = orientation.getVelocity(); euler_t angles = orientation.getEuler(); // Eigen::Matrix gravity = Eigen::Matrix::Zero(); - gravity(0, 0) = -9.81; - float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; h = height_full; + gravity(0, 0) = -9.81; + float m = mass_sustainer; + float h = height_sustainer; + if (fsm < FSMState::STATE_BURNOUT) + { + m = mass_full; + h = height_full; } - float w_x = omega.vx; float w_y = omega.vy; float w_z = omega.vz; - - float J_x = 0.5 * m * r * r; - float J_y = (1/3) * m * h * h + 0.25 * m * r * r; - float J_z = J_y; - - float vel_mag_squared = x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0); + float w_x = omega.vx; + float w_y = omega.vy; + float w_z = omega.vz; + float J_x = 0.5 * m * r * r; + float J_y = (1 / 3) * m * h * h + 0.25 * m * r * r; + float J_z = J_y; - float Fax = -0.5*rho*(vel_mag_squared)*float(Ca)*(pi*r*r); - float Fay = 0; float Faz = 0; + float vel_mag_squared = x_k(1, 0) * x_k(1, 0) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0); + float Fax = -0.5 * rho * (vel_mag_squared) * float(Ca) * (pi * r * r); + float Fay = 0; + float Faz = 0; - Eigen::Matrix *Fg_body = (GlobalToBody(angles, gravity)); - float Fgx = (*Fg_body)(0,0); float Fgy = (*Fg_body)(1,0); float Fgz = (*Fg_body)(2,0); + Eigen::Matrix *Fg_body = (GlobalToBody(angles)); + float Fgx = (*Fg_body)(0, 0); + float Fgy = (*Fg_body)(1, 0); + float Fgz = (*Fg_body)(2, 0); delete Fg_body; - Eigen::Matrix * T = EKF::getThrust(stage_timestamp, angles, fsm); - float Ftx = (*T)(0, 0); float Fty = (*T)(1, 0); float Ftz = (*T)(2, 0); + Eigen::Matrix *T = EKF::getThrust(stage_timestamp, angles, fsm); + float Ftx = (*T)(0, 0); + float Fty = (*T)(1, 0); + float Ftz = (*T)(2, 0); delete T; - - xdot << x_k(1,0), - (Fax + Ftx + Fgx) / m - (w_y * x_k(7,0) - w_z * x_k(4,0)), - 1.0, - - - x_k(4,0), - (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), - 1.0, - + xdot << x_k(1, 0), + (Fax + Ftx + Fgx) / m - (w_y * x_k(7, 0) - w_z * x_k(4, 0)), + 1.0, - x_k(7,0), - (Faz + Ftz + Fgz) / m - (w_x * x_k(4,0) - w_y * x_k(1,0)), - 1.0; + x_k(4, 0), + (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7, 0)), + 1.0, + x_k(7, 0), + (Faz + Ftz + Fgz) / m - (w_x * x_k(4, 0) - w_y * x_k(1, 0)), + 1.0; x_priori = (xdot * dt) + x_k; setF(dt, fsm, w_x, w_y, w_z); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } - /** * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim */ -float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) { +float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) +{ return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); } - /** * @brief Returns the approximate thrust force from the motor given the thurst curve * @@ -333,14 +316,19 @@ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. * The thrust force is then rotated into the body frame using the BodyToGlobal function. */ -Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { +Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) +{ float interpolatedValue = 0; - if (FSM_state >= STATE_FIRST_BOOST){ - if (FSM_state < FSMState::STATE_BURNOUT ) { + if (FSM_state >= STATE_FIRST_BOOST) + { + if (FSM_state < FSMState::STATE_BURNOUT) + { // first stage - if(timestamp >= 0.009){ + if (timestamp >= 0.009) + { auto it = O5500X_data.lower_bound(timestamp); - if (it != O5500X_data.end()) { + if (it != O5500X_data.end()) + { float x0 = it->first; float y0 = it->second; ++it; @@ -349,12 +337,15 @@ Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMS interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); } } - } - else { - if (timestamp >= 0.083){ + } + else + { + if (timestamp >= 0.083) + { // second stage auto it = moonburner_data.lower_bound(timestamp); - if (it != moonburner_data.end()) { + if (it != moonburner_data.end()) + { float x0 = it->first; float y0 = it->second; ++it; @@ -365,12 +356,11 @@ Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMS } } } - Eigen::Matrix interpolatedVector; - interpolatedVector(0,0) = interpolatedValue; + Eigen::Matrix *interpolatedVector = new Eigen::Matrix(Eigen::Matrix::Zero()); + interpolatedVector(0, 0) = interpolatedValue; return BodyToGlobal(angles, interpolatedVector); } - /** * @brief Update Kalman Gain and state estimate with current sensor data * @@ -379,41 +369,41 @@ Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMS * the new sensor data is. After updating the gain, the state estimate is updated. * */ -void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { - if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { +void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) +{ + if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) + { float sum = 0; float data[10]; alt_buffer.readSlice(data, 0, 10); - for (float i : data) { + for (float i : data) + { sum += i; } KalmanState kalman_state = (KalmanState){sum / 10.0f, 0, 0, 0, 0, 0, 0, 0, 0}; setState(kalman_state); - } else if (FSM_state >= FSMState::STATE_APOGEE) { + } + else if (FSM_state >= FSMState::STATE_APOGEE) + { H(1, 2) = 0; } - Eigen::Matrix S_k = Eigen::Matrix::Zero(); S_k = (((H * P_priori * H.transpose()) + R)).inverse(); Eigen::Matrix identity = Eigen::Matrix::Identity(); K = (P_priori * H.transpose()) * S_k; - // Sensor Measurements - Eigen::Matrix accel = Eigen::Matrix::Zero(); - - accel(0, 0) = acceleration.az - 0.045; - accel(1, 0) = acceleration.ay - 0.065; - accel(2, 0) = -acceleration.ax - 0.06; + Eigen::Matrix *accel = new Eigen::Matrix(Eigen::Matrix::Zero()); + (*accel)(0, 0) = acceleration.az - 0.045; + (*accel)(1, 0) = acceleration.ay - 0.065; + (*accel)(2, 0) = -acceleration.ax - 0.06; euler_t angles = orientation.getEuler(); angles.yaw = -angles.yaw; - - Eigen::Matrix * acc = (BodyToGlobal(angles, accel)); - + Eigen::Matrix *acc = (BodyToGlobal(angles, accel)); y_k(1, 0) = ((*acc)(0)) * 9.81 - 9.81; y_k(2, 0) = ((*acc)(1)) * 9.81; @@ -424,13 +414,10 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori y_k(0, 0) = barometer.altitude; alt_buffer.push(barometer.altitude); - - - // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); P_k = (identity - K * H) * P_priori; - + kalman_state.state_est_pos_x = x_k(0, 0); kalman_state.state_est_vel_x = x_k(1, 0); kalman_state.state_est_accel_x = x_k(2, 0); @@ -441,13 +428,11 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori kalman_state.state_est_vel_z = x_k(7, 0); kalman_state.state_est_accel_z = x_k(8, 0); - - state.position = (Position){kalman_state.state_est_pos_x,kalman_state.state_est_pos_y,kalman_state.state_est_pos_z}; - state.velocity = (Velocity){kalman_state.state_est_vel_x,kalman_state.state_est_vel_y,kalman_state.state_est_vel_z}; - state.acceleration = (Acceleration){kalman_state.state_est_accel_x,kalman_state.state_est_accel_y,kalman_state.state_est_accel_z}; + state.position = (Position){kalman_state.state_est_pos_x, kalman_state.state_est_pos_y, kalman_state.state_est_pos_z}; + state.velocity = (Velocity){kalman_state.state_est_vel_x, kalman_state.state_est_vel_y, kalman_state.state_est_vel_z}; + state.acceleration = (Acceleration){kalman_state.state_est_accel_x, kalman_state.state_est_accel_y, kalman_state.state_est_accel_z}; } - /** * @brief Run Kalman filter calculations as long as FSM has passed IDLE * @@ -458,9 +443,12 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori * @param &orientation Current orientation * @param current_state Current FSM_state */ -void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) { - if (FSM_state >= FSMState::STATE_IDLE) { - if (FSM_state != last_fsm) { +void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) +{ + if (FSM_state >= FSMState::STATE_IDLE) + { + if (FSM_state != last_fsm) + { stage_timestamp = 0; } stage_timestamp += dt; @@ -472,23 +460,23 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati } } - /** * @brief Getter for state X * * @return the current state, see sensor_data.h for KalmanData */ -KalmanData EKF::getState() { +KalmanData EKF::getState() +{ return state; } - /** * @brief Sets state vector x * * @param state Wanted state vector */ -void EKF::setState(KalmanState state) { +void EKF::setState(KalmanState state) +{ this->state.position.px = state.state_est_pos_x; this->state.position.py = state.state_est_pos_y; this->state.position.pz = state.state_est_pos_z; @@ -500,7 +488,6 @@ void EKF::setState(KalmanState state) { this->state.velocity.vz = state.state_est_vel_z; } - /** * @brief Sets the Q matrix given time step and spectral density. * @@ -510,7 +497,8 @@ void EKF::setState(KalmanState state) { * The Q matrix is the covariance matrix for the process noise and is * updated based on the time taken per cycle of the Kalman Filter Thread. */ -void EKF::setQ(float dt, float sd) { +void EKF::setQ(float dt, float sd) +{ Q(0, 0) = pow(dt, 5) / 20; Q(0, 1) = pow(dt, 4) / 8; Q(0, 2) = pow(dt, 3) / 6; @@ -530,7 +518,6 @@ void EKF::setQ(float dt, float sd) { Q(5, 3) = Q(3, 5); Q(5, 4) = Q(4, 5); - Q(6, 6) = pow(dt, 5) / 20; Q(6, 7) = pow(dt, 4) / 8; Q(6, 8) = pow(dt, 3) / 6; @@ -541,11 +528,9 @@ void EKF::setQ(float dt, float sd) { Q(8, 6) = Q(6, 8); Q(8, 7) = Q(7, 8); - Q *= sd; } - /** * @brief Converts a vector in the body frame to the global frame * @@ -553,22 +538,22 @@ void EKF::setQ(float dt, float sd) { * @param body_vect Vector for rotation in the body frame * @return Eigen::Matrix Rotated vector in the global frame */ -Eigen::Matrix * EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { +Eigen::Matrix *EKF::BodyToGlobal(euler_t angles, Eigen::Matrix *body_vect) +{ Eigen::Matrix3f roll, pitch, yaw; roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; - - Eigen::Matrix * global = new Eigen::Matrix(yaw * pitch * roll * body_vect); + + Eigen::Matrix *global = new Eigen::Matrix(yaw * pitch * roll * (*body_vect)); + delete body_vect; return global; } - /** * THIS IS A PLACEHOLDER FUNCTION SO WE CAN ABSTRACT FROM `kalman_filter.h` */ -void EKF::priori(){}; - +void EKF::priori() {}; /** * @brief Converts a vector in the global frame to the body frame @@ -579,19 +564,19 @@ void EKF::priori(){}; * @return Eigen::Matrix Rotated vector in the body frame * */ -Eigen::Matrix * EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { - +Eigen::Matrix *EKF::GlobalToBody(euler_t angles) +{ + Eigen::Matrix3f roll; roll << 1, 0, 0, 0, cos(angles.roll), -sin(angles.roll), 0, sin(angles.roll), cos(angles.roll); Eigen::Matrix3f pitch; - pitch << cos(angles.pitch), 0, sin(angles.pitch), 0, 1, 0,-sin(angles.pitch), 0, cos(angles.pitch); + pitch << cos(angles.pitch), 0, sin(angles.pitch), 0, 1, 0, -sin(angles.pitch), 0, cos(angles.pitch); Eigen::Matrix3f yaw; yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1; Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; (rotation_matrix).transpose(); - Eigen::Matrix * to_return = new Eigen::Matrix(rotation_matrix*world_vector); + Eigen::Matrix *to_return = new Eigen::Matrix(rotation_matrix * gravity); return to_return; - } /** @@ -603,17 +588,17 @@ Eigen::Matrix * EKF::GlobalToBody(euler_t angles, const Eigen::Matr * by how the states change over time and also depends on the * current state of the rocket. */ -void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { +void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) +{ Eigen::Matrix w = Eigen::Matrix::Zero(); w(0, 0) = wx; w(1, 0) = wy; w(2, 0) = wz; - F_mat(0,1) = 1; - F_mat(3,4) = 1; - F_mat(6,7) = 1; + F_mat(0, 1) = 1; + F_mat(3, 4) = 1; + F_mat(6, 7) = 1; - - float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); + float velocity_magnitude = pow(x_k(1, 0) * x_k(1, 0) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0), 0.5); float mach = velocity_magnitude / a; int index = std::round(mach / 0.04); index = std::clamp(index, 0, (int)AERO_DATA_SIZE - 1); @@ -624,28 +609,23 @@ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; - h= height_full; + if (fsm < FSMState::STATE_BURNOUT) + { + m = mass_full; + h = height_full; } + F_mat(1, 1) = -pi * Ca * r * r * rho * x_k(1, 0) / m; + F_mat(1, 4) = -pi * Ca * r * r * rho * x_k(4, 0) / m + w(2, 0); + F_mat(1, 7) = -pi * Ca * r * r * rho * x_k(7, 0) / m - w(1, 0); - F_mat(1,1) = -pi * Ca * r * r * rho * x_k(1,0) / m; - F_mat(1,4) = -pi * Ca * r * r * rho * x_k(4,0) / m + w(2,0); - F_mat(1,7) = -pi * Ca * r * r * rho * x_k(7,0) / m - w(1,0); - - - F_mat(4,1) = pi * Cn * r * r * rho * x_k(1,0) / m - w(2,0); - F_mat(4,4) = pi * Cn * r * r * rho * x_k(2,0) / m + w(0,0); - F_mat(4,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + F_mat(4, 1) = pi * Cn * r * r * rho * x_k(1, 0) / m - w(2, 0); + F_mat(4, 4) = pi * Cn * r * r * rho * x_k(2, 0) / m + w(0, 0); + F_mat(4, 7) = pi * Cn * r * r * rho * x_k(3, 0) / m; - - F_mat(7,1) = pi * Cn * r * r * rho * x_k(1,0) / m + w(1,0); - F_mat(7,4) = pi * Cn * r * r * rho * x_k(2,0) / m - w(0,0); - F_mat(7,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + F_mat(7, 1) = pi * Cn * r * r * rho * x_k(1, 0) / m + w(1, 0); + F_mat(7, 4) = pi * Cn * r * r * rho * x_k(2, 0) / m - w(0, 0); + F_mat(7, 7) = pi * Cn * r * r * rho * x_k(3, 0) / m; } EKF ekf; - - - diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 2c8f609e..5ea41a6c 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -20,8 +20,8 @@ class EKF : public KalmanFilter void setQ(float dt, float sd); void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z); Eigen::Matrix *getThrust(float timestamp, euler_t angles, FSMState FSM_state); - Eigen::Matrix *BodyToGlobal(euler_t angles, Eigen::Matrix x_k); - Eigen::Matrix *GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); + Eigen::Matrix *BodyToGlobal(euler_t angles, Eigen::Matrix *x_k); + Eigen::Matrix *GlobalToBody(euler_t angles); KalmanData getState() override; void setState(KalmanState state) override; From 46305328503a69e8d00e80e6da30d1a70b6b114b Mon Sep 17 00:00:00 2001 From: aneeshg5 Date: Sat, 1 Feb 2025 16:42:58 -0600 Subject: [PATCH 12/22] Create LookUp.csv --- MIDAS/src/gnc/LookUp.csv | 301 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 301 insertions(+) create mode 100644 MIDAS/src/gnc/LookUp.csv diff --git a/MIDAS/src/gnc/LookUp.csv b/MIDAS/src/gnc/LookUp.csv new file mode 100644 index 00000000..64091fe2 --- /dev/null +++ b/MIDAS/src/gnc/LookUp.csv @@ -0,0 +1,301 @@ +Mach Alpha CA Power-On CN CP +0.01 0 1.21363468 0 130.8434828 +0.02 0 1.059250667 0 130.8434828 +0.03 0 0.98198134 0 130.8434828 +0.04 0 0.932102339 0 130.8434828 +0.05 0 0.895957353 0 130.8434828 +0.06 0 0.867951263 0 130.8434828 +0.07 0 0.84527952 0 130.8434828 +0.08 0 0.826349122 0 130.8434828 +0.09 0 0.810174226 0 130.8434828 +0.1 0 0.796105211 0 130.8434828 +0.11 0 0.783692696 0 130.8434828 +0.12 0 0.772614241 0 130.8434828 +0.13 0 0.762630651 0 130.8434828 +0.14 0 0.753560431 0 130.8434828 +0.15 0 0.745262372 0 130.8434828 +0.16 0 0.737625023 0 130.8434828 +0.17 0 0.73055866 0 130.8434828 +0.18 0 0.723990282 0 130.8434828 +0.19 0 0.71785964 0 130.8434828 +0.2 0 0.712116363 0 130.8434828 +0.21 0 0.706718296 0 130.8434828 +0.22 0 0.701629416 0 130.8434828 +0.23 0 0.696819031 0 130.8434828 +0.24 0 0.69226057 0 130.8434828 +0.25 0 0.687931087 0 130.8434828 +0.26 0 0.684291729 0 130.8434828 +0.27 0 0.68083682 0 130.8434828 +0.28 0 0.677551341 0 130.8434828 +0.29 0 0.674421876 0 130.8434828 +0.3 0 0.671436546 0 130.8434828 +0.31 0 0.668584695 0 130.8434828 +0.32 0 0.665856819 0 130.8434828 +0.33 0 0.663244309 0 130.8434828 +0.34 0 0.660739414 0 130.8434828 +0.35 0 0.65833509 0 130.8434828 +0.36 0 0.656024994 0 130.8434828 +0.37 0 0.653803302 0 130.8434828 +0.38 0 0.651664687 0 130.8434828 +0.39 0 0.649604303 0 130.8434828 +0.4 0 0.647617643 0 130.8434828 +0.41 0 0.645700676 0 130.8434828 +0.42 0 0.643849546 0 130.8434828 +0.43 0 0.642060799 0 130.8434828 +0.44 0 0.640331201 0 130.8434828 +0.45 0 0.63865774 0 130.8434828 +0.46 0 0.637037674 0 130.8434828 +0.47 0 0.635468329 0 130.8434828 +0.48 0 0.633947375 0 130.8434828 +0.49 0 0.632472504 0 130.8434828 +0.5 0 0.631041685 0 130.8434828 +0.51 0 0.629652905 0 130.8434828 +0.52 0 0.628304284 0 130.8434828 +0.53 0 0.626994116 0 130.8434828 +0.54 0 0.625720828 0 130.8434828 +0.55 0 0.624482821 0 130.8434828 +0.56 0 0.62327875 0 130.8434828 +0.57 0 0.62210716 0 130.8434828 +0.58 0 0.620966804 0 130.8434828 +0.59 0 0.619354403 0 130.8434828 +0.6 0 0.617770878 0 130.8434828 +0.61 0 0.616961561 0 130.8434828 +0.62 0 0.616148054 0 130.8434828 +0.63 0 0.615356403 0 130.8434828 +0.64 0 0.614585797 0 130.8434828 +0.65 0 0.613835481 0 130.8434828 +0.66 0 0.613104659 0 130.8434828 +0.67 0 0.61239275 0 130.8434828 +0.68 0 0.611699077 0 130.8434828 +0.69 0 0.611022925 0 130.8434828 +0.7 0 0.6103638 0 130.8434828 +0.71 0 0.609721091 0 130.8434828 +0.72 0 0.609094272 0 130.8434828 +0.73 0 0.60848281 0 130.8434828 +0.74 0 0.607886229 0 130.8434828 +0.75 0 0.607304086 0 130.8434828 +0.76 0 0.606735837 0 130.8434828 +0.77 0 0.606181158 0 130.8434828 +0.78 0 0.605639576 0 130.8434828 +0.79 0 0.605110696 0 130.8434828 +0.8 0 0.60459417 0 130.8434828 +0.81 0 0.604089608 0 130.8434828 +0.82 0 0.603596698 0 130.8434828 +0.83 0 0.603115046 0 130.8434828 +0.84 0 0.60264439 0 130.8434828 +0.85 0 0.602184378 0 130.8434828 +0.86 0 0.601734704 0 130.8434828 +0.87 0 0.601295144 0 130.8434828 +0.88 0 0.600865377 0 130.8434828 +0.89 0 0.600445159 0 130.8434828 +0.9 0 0.600034218 0 130.8434828 +0.91 0 0.604114451 0 131.0711106 +0.92 0 0.616355149 0 131.2987383 +0.93 0 0.628985539 0 131.5263661 +0.94 0 0.635885271 0 131.7539938 +0.95 0 0.642785003 0 131.9816215 +0.96 0 0.649684735 0 132.2092493 +0.97 0 0.656584467 0 132.436877 +0.98 0 0.663484199 0 132.6645048 +0.99 0 0.670383931 0 132.8921325 +1 0 0.677283664 0 133.1197603 +0.01 2 1.215114659 0.805897084 128.4528665 +0.02 2 1.060542381 0.805897084 128.4528665 +0.03 2 0.983178827 0.805897084 128.4528665 +0.04 2 0.933239001 0.805897084 128.4528665 +0.05 2 0.897049937 0.805897084 128.4528665 +0.06 2 0.869009695 0.805897084 128.4528665 +0.07 2 0.846310305 0.805897084 128.4528665 +0.08 2 0.827356822 0.805897084 128.4528665 +0.09 2 0.811162201 0.805897084 128.4528665 +0.1 2 0.79707603 0.805897084 128.4528665 +0.11 2 0.784648378 0.805897084 128.4528665 +0.12 2 0.773556413 0.805897084 128.4528665 +0.13 2 0.763560649 0.805897084 128.4528665 +0.14 2 0.754479368 0.805897084 128.4528665 +0.15 2 0.74617119 0.805897084 128.4528665 +0.16 2 0.738524527 0.805897084 128.4528665 +0.17 2 0.731449547 0.805897084 128.4528665 +0.18 2 0.72487316 0.805897084 128.4528665 +0.19 2 0.718735042 0.805897084 128.4528665 +0.2 2 0.712984761 0.805897084 128.4528665 +0.21 2 0.707580111 0.805897084 128.4528665 +0.22 2 0.702485025 0.805897084 128.4528665 +0.23 2 0.697668774 0.805897084 128.4528665 +0.24 2 0.693104754 0.805897084 128.4528665 +0.25 2 0.688769992 0.805897084 128.4528665 +0.26 2 0.685126196 0.805897084 128.4528665 +0.27 2 0.681667073 0.805897084 128.4528665 +0.28 2 0.678377587 0.805897084 128.4528665 +0.29 2 0.675244307 0.805897084 128.4528665 +0.3 2 0.672255336 0.805897084 128.4528665 +0.31 2 0.669400007 0.805897084 128.4528665 +0.32 2 0.666668805 0.805897084 128.4528665 +0.33 2 0.664053109 0.805897084 128.4528665 +0.34 2 0.661545159 0.805897084 128.4528665 +0.35 2 0.659137903 0.805897084 128.4528665 +0.36 2 0.656824991 0.805897084 128.4528665 +0.37 2 0.65460059 0.805897084 128.4528665 +0.38 2 0.652459366 0.805897084 128.4528665 +0.39 2 0.65039647 0.805897084 128.4528665 +0.4 2 0.648407387 0.805897084 128.4528665 +0.41 2 0.646488082 0.805897084 128.4528665 +0.42 2 0.644634695 0.805897084 128.4528665 +0.43 2 0.642843766 0.805897084 128.4528665 +0.44 2 0.64111206 0.805897084 128.4528665 +0.45 2 0.639436558 0.805897084 128.4528665 +0.46 2 0.637814516 0.805897084 128.4528665 +0.47 2 0.636243257 0.805897084 128.4528665 +0.48 2 0.634720449 0.805897084 128.4528665 +0.49 2 0.633243779 0.805897084 128.4528665 +0.5 2 0.631811215 0.805897084 128.4528665 +0.51 2 0.630420742 0.805897084 128.4528665 +0.52 2 0.629070476 0.805897084 128.4528665 +0.53 2 0.62775871 0.805897084 128.4528665 +0.54 2 0.626483869 0.805897084 128.4528665 +0.55 2 0.625244353 0.805897084 128.4528665 +0.56 2 0.624038814 0.805897084 128.4528665 +0.57 2 0.622865794 0.805897084 128.4528665 +0.58 2 0.621724048 0.805897084 128.4528665 +0.59 2 0.620109681 0.805897084 128.4528665 +0.6 2 0.618524225 0.805897084 128.4528665 +0.61 2 0.617713921 0.805897084 128.4528665 +0.62 2 0.616899422 0.805897084 128.4528665 +0.63 2 0.616106805 0.805897084 128.4528665 +0.64 2 0.61533526 0.805897084 128.4528665 +0.65 2 0.614584029 0.805897084 128.4528665 +0.66 2 0.613852316 0.805897084 128.4528665 +0.67 2 0.613139538 0.805897084 128.4528665 +0.68 2 0.61244502 0.805897084 128.4528665 +0.69 2 0.611768043 0.805897084 128.4528665 +0.7 2 0.611108114 0.805897084 128.4528665 +0.71 2 0.610464622 0.805897084 128.4528665 +0.72 2 0.609837038 0.805897084 128.4528665 +0.73 2 0.609224831 0.805897084 128.4528665 +0.74 2 0.608627522 0.805897084 128.4528665 +0.75 2 0.608044669 0.805897084 128.4528665 +0.76 2 0.607475727 0.805897084 128.4528665 +0.77 2 0.606920372 0.805897084 128.4528665 +0.78 2 0.60637813 0.805897084 128.4528665 +0.79 2 0.605848604 0.805897084 128.4528665 +0.8 2 0.605331448 0.805897084 128.4528665 +0.81 2 0.604826271 0.805897084 128.4528665 +0.82 2 0.60433276 0.805897084 128.4528665 +0.83 2 0.603850521 0.805897084 128.4528665 +0.84 2 0.603379291 0.805897084 128.4528665 +0.85 2 0.602918718 0.805897084 128.4528665 +0.86 2 0.602468496 0.805897084 128.4528665 +0.87 2 0.6020284 0.805897084 128.4528665 +0.88 2 0.601598109 0.805897084 128.4528665 +0.89 2 0.601177378 0.805897084 128.4528665 +0.9 2 0.600765936 0.805897084 128.4528665 +0.91 2 0.604851144 0.80173249 128.6489201 +0.92 2 0.61710677 0.797567897 128.8449737 +0.93 2 0.629752561 0.793403304 129.0410273 +0.94 2 0.636660707 0.789238711 129.2370808 +0.95 2 0.643568854 0.785074117 129.4331344 +0.96 2 0.650477 0.780909524 129.629188 +0.97 2 0.657385146 0.776744931 129.8252416 +0.98 2 0.664293292 0.772580338 130.0212952 +0.99 2 0.671201438 0.768415744 130.2173488 +1 2 0.678109584 0.764251151 130.4134023 +0.01 4 1.219569061 1.730270476 126.3950588 +0.02 4 1.064430148 1.730270476 126.3950588 +0.03 4 0.986782992 1.730270476 126.3950588 +0.04 4 0.936660095 1.730270476 126.3950588 +0.05 4 0.900338368 1.730270476 126.3950588 +0.06 4 0.872195336 1.730270476 126.3950588 +0.07 4 0.849412733 1.730270476 126.3950588 +0.08 4 0.830389771 1.730270476 126.3950588 +0.09 4 0.814135782 1.730270476 126.3950588 +0.1 4 0.799997974 1.730270476 126.3950588 +0.11 4 0.787524764 1.730270476 126.3950588 +0.12 4 0.776392138 1.730270476 126.3950588 +0.13 4 0.766359731 1.730270476 126.3950588 +0.14 4 0.75724516 1.730270476 126.3950588 +0.15 4 0.748906526 1.730270476 126.3950588 +0.16 4 0.741231832 1.730270476 126.3950588 +0.17 4 0.734130916 1.730270476 126.3950588 +0.18 4 0.72753042 1.730270476 126.3950588 +0.19 4 0.721369801 1.730270476 126.3950588 +0.2 4 0.71559844 1.730270476 126.3950588 +0.21 4 0.710173979 1.730270476 126.3950588 +0.22 4 0.705060215 1.730270476 126.3950588 +0.23 4 0.700226308 1.730270476 126.3950588 +0.24 4 0.695645557 1.730270476 126.3950588 +0.25 4 0.691294905 1.730270476 126.3950588 +0.26 4 0.687637751 1.730270476 126.3950588 +0.27 4 0.684165948 1.730270476 126.3950588 +0.28 4 0.680864403 1.730270476 126.3950588 +0.29 4 0.677719637 1.730270476 126.3950588 +0.3 4 0.674719709 1.730270476 126.3950588 +0.31 4 0.671853913 1.730270476 126.3950588 +0.32 4 0.669112698 1.730270476 126.3950588 +0.33 4 0.666487414 1.730270476 126.3950588 +0.34 4 0.66397027 1.730270476 126.3950588 +0.35 4 0.66155419 1.730270476 126.3950588 +0.36 4 0.659232798 1.730270476 126.3950588 +0.37 4 0.657000243 1.730270476 126.3950588 +0.38 4 0.65485117 1.730270476 126.3950588 +0.39 4 0.652780712 1.730270476 126.3950588 +0.4 4 0.650784338 1.730270476 126.3950588 +0.41 4 0.648857996 1.730270476 126.3950588 +0.42 4 0.646997815 1.730270476 126.3950588 +0.43 4 0.645200321 1.730270476 126.3950588 +0.44 4 0.643462266 1.730270476 126.3950588 +0.45 4 0.641780622 1.730270476 126.3950588 +0.46 4 0.640152634 1.730270476 126.3950588 +0.47 4 0.638575616 1.730270476 126.3950588 +0.48 4 0.637047225 1.730270476 126.3950588 +0.49 4 0.635565142 1.730270476 126.3950588 +0.5 4 0.634127327 1.730270476 126.3950588 +0.51 4 0.632731756 1.730270476 126.3950588 +0.52 4 0.631376541 1.730270476 126.3950588 +0.53 4 0.630059966 1.730270476 126.3950588 +0.54 4 0.628780452 1.730270476 126.3950588 +0.55 4 0.627536391 1.730270476 126.3950588 +0.56 4 0.626326433 1.730270476 126.3950588 +0.57 4 0.625149114 1.730270476 126.3950588 +0.58 4 0.624003182 1.730270476 126.3950588 +0.59 4 0.622382897 1.730270476 126.3950588 +0.6 4 0.620791629 1.730270476 126.3950588 +0.61 4 0.619978354 1.730270476 126.3950588 +0.62 4 0.61916087 1.730270476 126.3950588 +0.63 4 0.618365347 1.730270476 126.3950588 +0.64 4 0.617590973 1.730270476 126.3950588 +0.65 4 0.616836989 1.730270476 126.3950588 +0.66 4 0.616102593 1.730270476 126.3950588 +0.67 4 0.615387203 1.730270476 126.3950588 +0.68 4 0.614690138 1.730270476 126.3950588 +0.69 4 0.61401068 1.730270476 126.3950588 +0.7 4 0.613348332 1.730270476 126.3950588 +0.71 4 0.612702481 1.730270476 126.3950588 +0.72 4 0.612072596 1.730270476 126.3950588 +0.73 4 0.611458144 1.730270476 126.3950588 +0.74 4 0.610858646 1.730270476 126.3950588 +0.75 4 0.610273657 1.730270476 126.3950588 +0.76 4 0.609702629 1.730270476 126.3950588 +0.77 4 0.609145238 1.730270476 126.3950588 +0.78 4 0.608601008 1.730270476 126.3950588 +0.79 4 0.608069541 1.730270476 126.3950588 +0.8 4 0.60755049 1.730270476 126.3950588 +0.81 4 0.607043461 1.730270476 126.3950588 +0.82 4 0.606548141 1.730270476 126.3950588 +0.83 4 0.606064134 1.730270476 126.3950588 +0.84 4 0.605591176 1.730270476 126.3950588 +0.85 4 0.605128915 1.730270476 126.3950588 +0.86 4 0.604677042 1.730270476 126.3950588 +0.87 4 0.604235333 1.730270476 126.3950588 +0.88 4 0.603803464 1.730270476 126.3950588 +0.89 4 0.603381191 1.730270476 126.3950588 +0.9 4 0.602968241 1.730270476 126.3950588 +0.91 4 0.607068425 1.721941289 126.5659665 +0.92 4 0.619368977 1.713612103 126.7368741 +0.93 4 0.632061126 1.705282916 126.9077818 +0.94 4 0.638994596 1.696953729 127.0786894 +0.95 4 0.645928067 1.688624543 127.2495971 +0.96 4 0.652861537 1.680295356 127.4205048 +0.97 4 0.659795007 1.67196617 127.5914124 +0.98 4 0.666728477 1.663636983 127.7623201 +0.99 4 0.673661947 1.655307797 127.9332278 +1 4 0.680595417 1.64697861 128.1041354 From b01d67124395ae7feeacb99a7a1b9737c1075223 Mon Sep 17 00:00:00 2001 From: Shishir Date: Sat, 1 Feb 2025 21:26:20 -0600 Subject: [PATCH 13/22] rip yessir.cpp :( Co-authored-by: aneeshg5 Co-authored-by: divijgarg Co-authored-by: Keshav B --- MIDAS/src/gnc/LookUp.csv | 327 ++++----------------------------------- MIDAS/src/sensor_data.h | 6 + MIDAS/src/systems.cpp | 6 +- 3 files changed, 33 insertions(+), 306 deletions(-) diff --git a/MIDAS/src/gnc/LookUp.csv b/MIDAS/src/gnc/LookUp.csv index 64091fe2..cfd5768a 100644 --- a/MIDAS/src/gnc/LookUp.csv +++ b/MIDAS/src/gnc/LookUp.csv @@ -1,301 +1,26 @@ -Mach Alpha CA Power-On CN CP -0.01 0 1.21363468 0 130.8434828 -0.02 0 1.059250667 0 130.8434828 -0.03 0 0.98198134 0 130.8434828 -0.04 0 0.932102339 0 130.8434828 -0.05 0 0.895957353 0 130.8434828 -0.06 0 0.867951263 0 130.8434828 -0.07 0 0.84527952 0 130.8434828 -0.08 0 0.826349122 0 130.8434828 -0.09 0 0.810174226 0 130.8434828 -0.1 0 0.796105211 0 130.8434828 -0.11 0 0.783692696 0 130.8434828 -0.12 0 0.772614241 0 130.8434828 -0.13 0 0.762630651 0 130.8434828 -0.14 0 0.753560431 0 130.8434828 -0.15 0 0.745262372 0 130.8434828 -0.16 0 0.737625023 0 130.8434828 -0.17 0 0.73055866 0 130.8434828 -0.18 0 0.723990282 0 130.8434828 -0.19 0 0.71785964 0 130.8434828 -0.2 0 0.712116363 0 130.8434828 -0.21 0 0.706718296 0 130.8434828 -0.22 0 0.701629416 0 130.8434828 -0.23 0 0.696819031 0 130.8434828 -0.24 0 0.69226057 0 130.8434828 -0.25 0 0.687931087 0 130.8434828 -0.26 0 0.684291729 0 130.8434828 -0.27 0 0.68083682 0 130.8434828 -0.28 0 0.677551341 0 130.8434828 -0.29 0 0.674421876 0 130.8434828 -0.3 0 0.671436546 0 130.8434828 -0.31 0 0.668584695 0 130.8434828 -0.32 0 0.665856819 0 130.8434828 -0.33 0 0.663244309 0 130.8434828 -0.34 0 0.660739414 0 130.8434828 -0.35 0 0.65833509 0 130.8434828 -0.36 0 0.656024994 0 130.8434828 -0.37 0 0.653803302 0 130.8434828 -0.38 0 0.651664687 0 130.8434828 -0.39 0 0.649604303 0 130.8434828 -0.4 0 0.647617643 0 130.8434828 -0.41 0 0.645700676 0 130.8434828 -0.42 0 0.643849546 0 130.8434828 -0.43 0 0.642060799 0 130.8434828 -0.44 0 0.640331201 0 130.8434828 -0.45 0 0.63865774 0 130.8434828 -0.46 0 0.637037674 0 130.8434828 -0.47 0 0.635468329 0 130.8434828 -0.48 0 0.633947375 0 130.8434828 -0.49 0 0.632472504 0 130.8434828 -0.5 0 0.631041685 0 130.8434828 -0.51 0 0.629652905 0 130.8434828 -0.52 0 0.628304284 0 130.8434828 -0.53 0 0.626994116 0 130.8434828 -0.54 0 0.625720828 0 130.8434828 -0.55 0 0.624482821 0 130.8434828 -0.56 0 0.62327875 0 130.8434828 -0.57 0 0.62210716 0 130.8434828 -0.58 0 0.620966804 0 130.8434828 -0.59 0 0.619354403 0 130.8434828 -0.6 0 0.617770878 0 130.8434828 -0.61 0 0.616961561 0 130.8434828 -0.62 0 0.616148054 0 130.8434828 -0.63 0 0.615356403 0 130.8434828 -0.64 0 0.614585797 0 130.8434828 -0.65 0 0.613835481 0 130.8434828 -0.66 0 0.613104659 0 130.8434828 -0.67 0 0.61239275 0 130.8434828 -0.68 0 0.611699077 0 130.8434828 -0.69 0 0.611022925 0 130.8434828 -0.7 0 0.6103638 0 130.8434828 -0.71 0 0.609721091 0 130.8434828 -0.72 0 0.609094272 0 130.8434828 -0.73 0 0.60848281 0 130.8434828 -0.74 0 0.607886229 0 130.8434828 -0.75 0 0.607304086 0 130.8434828 -0.76 0 0.606735837 0 130.8434828 -0.77 0 0.606181158 0 130.8434828 -0.78 0 0.605639576 0 130.8434828 -0.79 0 0.605110696 0 130.8434828 -0.8 0 0.60459417 0 130.8434828 -0.81 0 0.604089608 0 130.8434828 -0.82 0 0.603596698 0 130.8434828 -0.83 0 0.603115046 0 130.8434828 -0.84 0 0.60264439 0 130.8434828 -0.85 0 0.602184378 0 130.8434828 -0.86 0 0.601734704 0 130.8434828 -0.87 0 0.601295144 0 130.8434828 -0.88 0 0.600865377 0 130.8434828 -0.89 0 0.600445159 0 130.8434828 -0.9 0 0.600034218 0 130.8434828 -0.91 0 0.604114451 0 131.0711106 -0.92 0 0.616355149 0 131.2987383 -0.93 0 0.628985539 0 131.5263661 -0.94 0 0.635885271 0 131.7539938 -0.95 0 0.642785003 0 131.9816215 -0.96 0 0.649684735 0 132.2092493 -0.97 0 0.656584467 0 132.436877 -0.98 0 0.663484199 0 132.6645048 -0.99 0 0.670383931 0 132.8921325 -1 0 0.677283664 0 133.1197603 -0.01 2 1.215114659 0.805897084 128.4528665 -0.02 2 1.060542381 0.805897084 128.4528665 -0.03 2 0.983178827 0.805897084 128.4528665 -0.04 2 0.933239001 0.805897084 128.4528665 -0.05 2 0.897049937 0.805897084 128.4528665 -0.06 2 0.869009695 0.805897084 128.4528665 -0.07 2 0.846310305 0.805897084 128.4528665 -0.08 2 0.827356822 0.805897084 128.4528665 -0.09 2 0.811162201 0.805897084 128.4528665 -0.1 2 0.79707603 0.805897084 128.4528665 -0.11 2 0.784648378 0.805897084 128.4528665 -0.12 2 0.773556413 0.805897084 128.4528665 -0.13 2 0.763560649 0.805897084 128.4528665 -0.14 2 0.754479368 0.805897084 128.4528665 -0.15 2 0.74617119 0.805897084 128.4528665 -0.16 2 0.738524527 0.805897084 128.4528665 -0.17 2 0.731449547 0.805897084 128.4528665 -0.18 2 0.72487316 0.805897084 128.4528665 -0.19 2 0.718735042 0.805897084 128.4528665 -0.2 2 0.712984761 0.805897084 128.4528665 -0.21 2 0.707580111 0.805897084 128.4528665 -0.22 2 0.702485025 0.805897084 128.4528665 -0.23 2 0.697668774 0.805897084 128.4528665 -0.24 2 0.693104754 0.805897084 128.4528665 -0.25 2 0.688769992 0.805897084 128.4528665 -0.26 2 0.685126196 0.805897084 128.4528665 -0.27 2 0.681667073 0.805897084 128.4528665 -0.28 2 0.678377587 0.805897084 128.4528665 -0.29 2 0.675244307 0.805897084 128.4528665 -0.3 2 0.672255336 0.805897084 128.4528665 -0.31 2 0.669400007 0.805897084 128.4528665 -0.32 2 0.666668805 0.805897084 128.4528665 -0.33 2 0.664053109 0.805897084 128.4528665 -0.34 2 0.661545159 0.805897084 128.4528665 -0.35 2 0.659137903 0.805897084 128.4528665 -0.36 2 0.656824991 0.805897084 128.4528665 -0.37 2 0.65460059 0.805897084 128.4528665 -0.38 2 0.652459366 0.805897084 128.4528665 -0.39 2 0.65039647 0.805897084 128.4528665 -0.4 2 0.648407387 0.805897084 128.4528665 -0.41 2 0.646488082 0.805897084 128.4528665 -0.42 2 0.644634695 0.805897084 128.4528665 -0.43 2 0.642843766 0.805897084 128.4528665 -0.44 2 0.64111206 0.805897084 128.4528665 -0.45 2 0.639436558 0.805897084 128.4528665 -0.46 2 0.637814516 0.805897084 128.4528665 -0.47 2 0.636243257 0.805897084 128.4528665 -0.48 2 0.634720449 0.805897084 128.4528665 -0.49 2 0.633243779 0.805897084 128.4528665 -0.5 2 0.631811215 0.805897084 128.4528665 -0.51 2 0.630420742 0.805897084 128.4528665 -0.52 2 0.629070476 0.805897084 128.4528665 -0.53 2 0.62775871 0.805897084 128.4528665 -0.54 2 0.626483869 0.805897084 128.4528665 -0.55 2 0.625244353 0.805897084 128.4528665 -0.56 2 0.624038814 0.805897084 128.4528665 -0.57 2 0.622865794 0.805897084 128.4528665 -0.58 2 0.621724048 0.805897084 128.4528665 -0.59 2 0.620109681 0.805897084 128.4528665 -0.6 2 0.618524225 0.805897084 128.4528665 -0.61 2 0.617713921 0.805897084 128.4528665 -0.62 2 0.616899422 0.805897084 128.4528665 -0.63 2 0.616106805 0.805897084 128.4528665 -0.64 2 0.61533526 0.805897084 128.4528665 -0.65 2 0.614584029 0.805897084 128.4528665 -0.66 2 0.613852316 0.805897084 128.4528665 -0.67 2 0.613139538 0.805897084 128.4528665 -0.68 2 0.61244502 0.805897084 128.4528665 -0.69 2 0.611768043 0.805897084 128.4528665 -0.7 2 0.611108114 0.805897084 128.4528665 -0.71 2 0.610464622 0.805897084 128.4528665 -0.72 2 0.609837038 0.805897084 128.4528665 -0.73 2 0.609224831 0.805897084 128.4528665 -0.74 2 0.608627522 0.805897084 128.4528665 -0.75 2 0.608044669 0.805897084 128.4528665 -0.76 2 0.607475727 0.805897084 128.4528665 -0.77 2 0.606920372 0.805897084 128.4528665 -0.78 2 0.60637813 0.805897084 128.4528665 -0.79 2 0.605848604 0.805897084 128.4528665 -0.8 2 0.605331448 0.805897084 128.4528665 -0.81 2 0.604826271 0.805897084 128.4528665 -0.82 2 0.60433276 0.805897084 128.4528665 -0.83 2 0.603850521 0.805897084 128.4528665 -0.84 2 0.603379291 0.805897084 128.4528665 -0.85 2 0.602918718 0.805897084 128.4528665 -0.86 2 0.602468496 0.805897084 128.4528665 -0.87 2 0.6020284 0.805897084 128.4528665 -0.88 2 0.601598109 0.805897084 128.4528665 -0.89 2 0.601177378 0.805897084 128.4528665 -0.9 2 0.600765936 0.805897084 128.4528665 -0.91 2 0.604851144 0.80173249 128.6489201 -0.92 2 0.61710677 0.797567897 128.8449737 -0.93 2 0.629752561 0.793403304 129.0410273 -0.94 2 0.636660707 0.789238711 129.2370808 -0.95 2 0.643568854 0.785074117 129.4331344 -0.96 2 0.650477 0.780909524 129.629188 -0.97 2 0.657385146 0.776744931 129.8252416 -0.98 2 0.664293292 0.772580338 130.0212952 -0.99 2 0.671201438 0.768415744 130.2173488 -1 2 0.678109584 0.764251151 130.4134023 -0.01 4 1.219569061 1.730270476 126.3950588 -0.02 4 1.064430148 1.730270476 126.3950588 -0.03 4 0.986782992 1.730270476 126.3950588 -0.04 4 0.936660095 1.730270476 126.3950588 -0.05 4 0.900338368 1.730270476 126.3950588 -0.06 4 0.872195336 1.730270476 126.3950588 -0.07 4 0.849412733 1.730270476 126.3950588 -0.08 4 0.830389771 1.730270476 126.3950588 -0.09 4 0.814135782 1.730270476 126.3950588 -0.1 4 0.799997974 1.730270476 126.3950588 -0.11 4 0.787524764 1.730270476 126.3950588 -0.12 4 0.776392138 1.730270476 126.3950588 -0.13 4 0.766359731 1.730270476 126.3950588 -0.14 4 0.75724516 1.730270476 126.3950588 -0.15 4 0.748906526 1.730270476 126.3950588 -0.16 4 0.741231832 1.730270476 126.3950588 -0.17 4 0.734130916 1.730270476 126.3950588 -0.18 4 0.72753042 1.730270476 126.3950588 -0.19 4 0.721369801 1.730270476 126.3950588 -0.2 4 0.71559844 1.730270476 126.3950588 -0.21 4 0.710173979 1.730270476 126.3950588 -0.22 4 0.705060215 1.730270476 126.3950588 -0.23 4 0.700226308 1.730270476 126.3950588 -0.24 4 0.695645557 1.730270476 126.3950588 -0.25 4 0.691294905 1.730270476 126.3950588 -0.26 4 0.687637751 1.730270476 126.3950588 -0.27 4 0.684165948 1.730270476 126.3950588 -0.28 4 0.680864403 1.730270476 126.3950588 -0.29 4 0.677719637 1.730270476 126.3950588 -0.3 4 0.674719709 1.730270476 126.3950588 -0.31 4 0.671853913 1.730270476 126.3950588 -0.32 4 0.669112698 1.730270476 126.3950588 -0.33 4 0.666487414 1.730270476 126.3950588 -0.34 4 0.66397027 1.730270476 126.3950588 -0.35 4 0.66155419 1.730270476 126.3950588 -0.36 4 0.659232798 1.730270476 126.3950588 -0.37 4 0.657000243 1.730270476 126.3950588 -0.38 4 0.65485117 1.730270476 126.3950588 -0.39 4 0.652780712 1.730270476 126.3950588 -0.4 4 0.650784338 1.730270476 126.3950588 -0.41 4 0.648857996 1.730270476 126.3950588 -0.42 4 0.646997815 1.730270476 126.3950588 -0.43 4 0.645200321 1.730270476 126.3950588 -0.44 4 0.643462266 1.730270476 126.3950588 -0.45 4 0.641780622 1.730270476 126.3950588 -0.46 4 0.640152634 1.730270476 126.3950588 -0.47 4 0.638575616 1.730270476 126.3950588 -0.48 4 0.637047225 1.730270476 126.3950588 -0.49 4 0.635565142 1.730270476 126.3950588 -0.5 4 0.634127327 1.730270476 126.3950588 -0.51 4 0.632731756 1.730270476 126.3950588 -0.52 4 0.631376541 1.730270476 126.3950588 -0.53 4 0.630059966 1.730270476 126.3950588 -0.54 4 0.628780452 1.730270476 126.3950588 -0.55 4 0.627536391 1.730270476 126.3950588 -0.56 4 0.626326433 1.730270476 126.3950588 -0.57 4 0.625149114 1.730270476 126.3950588 -0.58 4 0.624003182 1.730270476 126.3950588 -0.59 4 0.622382897 1.730270476 126.3950588 -0.6 4 0.620791629 1.730270476 126.3950588 -0.61 4 0.619978354 1.730270476 126.3950588 -0.62 4 0.61916087 1.730270476 126.3950588 -0.63 4 0.618365347 1.730270476 126.3950588 -0.64 4 0.617590973 1.730270476 126.3950588 -0.65 4 0.616836989 1.730270476 126.3950588 -0.66 4 0.616102593 1.730270476 126.3950588 -0.67 4 0.615387203 1.730270476 126.3950588 -0.68 4 0.614690138 1.730270476 126.3950588 -0.69 4 0.61401068 1.730270476 126.3950588 -0.7 4 0.613348332 1.730270476 126.3950588 -0.71 4 0.612702481 1.730270476 126.3950588 -0.72 4 0.612072596 1.730270476 126.3950588 -0.73 4 0.611458144 1.730270476 126.3950588 -0.74 4 0.610858646 1.730270476 126.3950588 -0.75 4 0.610273657 1.730270476 126.3950588 -0.76 4 0.609702629 1.730270476 126.3950588 -0.77 4 0.609145238 1.730270476 126.3950588 -0.78 4 0.608601008 1.730270476 126.3950588 -0.79 4 0.608069541 1.730270476 126.3950588 -0.8 4 0.60755049 1.730270476 126.3950588 -0.81 4 0.607043461 1.730270476 126.3950588 -0.82 4 0.606548141 1.730270476 126.3950588 -0.83 4 0.606064134 1.730270476 126.3950588 -0.84 4 0.605591176 1.730270476 126.3950588 -0.85 4 0.605128915 1.730270476 126.3950588 -0.86 4 0.604677042 1.730270476 126.3950588 -0.87 4 0.604235333 1.730270476 126.3950588 -0.88 4 0.603803464 1.730270476 126.3950588 -0.89 4 0.603381191 1.730270476 126.3950588 -0.9 4 0.602968241 1.730270476 126.3950588 -0.91 4 0.607068425 1.721941289 126.5659665 -0.92 4 0.619368977 1.713612103 126.7368741 -0.93 4 0.632061126 1.705282916 126.9077818 -0.94 4 0.638994596 1.696953729 127.0786894 -0.95 4 0.645928067 1.688624543 127.2495971 -0.96 4 0.652861537 1.680295356 127.4205048 -0.97 4 0.659795007 1.67196617 127.5914124 -0.98 4 0.666728477 1.663636983 127.7623201 -0.99 4 0.673661947 1.655307797 127.9332278 -1 4 0.680595417 1.64697861 128.1041354 +Mach,Alpha,CA Power-On,CN,CP +0.04,4,1.332142905,1.1827808455016526632575949416,60.826787196815171703275113218 +0.08,4,1.326587387,1.1827808455016526632575949416,60.826787196815171703275113218 +0.12,4,1.31558627,1.1827808455016526632575949416,60.826787196815171703275113218 +0.16,4,1.306063953,1.1827808455016526632575949416,60.826787196815171703275113218 +0.20,4,1.298117084,1.1827808455016526632575949416,60.826787196815171703275113218 +0.24,4,1.291411025,1.1827808455016526632575949416,60.826787196815171703275113218 +0.28,4,1.290279857,1.1827808455016526632575949416,60.826787196815171703275113218 +0.32,4,1.291431043,1.1827808455016526632575949416,60.826787196815171703275113218 +0.36,4,1.293170653,1.1827808455016526632575949416,60.826787196815171703275113218 +0.40,4,1.295385827,1.1827808455016526632575949416,60.826787196815171703275113218 +0.44,4,1.297991738,1.1827808455016526632575949416,60.826787196815171703275113218 +0.48,4,1.300924032,1.1827808455016526632575949416,60.826787196815171703275113218 +0.52,4,1.304132086,1.1827808455016526632575949416,60.826787196815171703275113218 +0.56,4,1.309039395,1.1827808455016526632575949416,60.826787196815171703275113218 +0.60,4,1.314605487,1.1827808455016526632575949416,60.826787196815171703275113218 +0.64,4,1.330699437,1.1827808455016526632575949416,60.826787196815171703275113218 +0.68,4,1.346695167,1.1827808455016526632575949416,60.826787196815171703275113218 +0.72,4,1.362693183,1.1827808455016526632575949416,60.826787196815171703275113218 +0.76,4,1.378693074,1.1827808455016526632575949416,60.826787196815171703275113218 +0.80,4,1.394695194,1.1827808455016526632575949416,60.826787196815171703275113218 +0.84,4,1.41069913,1.1827808455016526632575949416,60.826787196815171703275113218 +0.88,4,1.426705046,1.1827808455016526632575949416,60.826787196815171703275113218 +0.92,4,1.473732816,1.218959468,60.91848997 +0.96,4,1.582395672,1.291316713,61.10189551 +1.00,4,1.681494886,1.363673958,61.28530105 diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/sensor_data.h index 5b12011c..c82a0a12 100644 --- a/MIDAS/src/sensor_data.h +++ b/MIDAS/src/sensor_data.h @@ -201,7 +201,13 @@ struct Orientation { return euler; } + Velocity orientation_velocity; + + Velocity getVelocity() const { + return orientation_velocity; + } + Acceleration orientation_acceleration; Acceleration linear_acceleration; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 699c94e4..a4a8f44a 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -181,10 +181,6 @@ DECLARE_THREAD(buzzer, RocketSystems* arg) { } DECLARE_THREAD(kalman, RocketSystems* arg) { - // Orientation initial_orientation = arg->rocket_data.orientation.getRecent(); - // Barometer initial_barom_buf = arg->rocket_data.barometer.getRecent(); - // LowGData initial_accelerometer = arg->rocket_data.low_g.getRecent(); - //yessir.initialize(initial_orientation, initial_barom_buf, initial_accelerations); yessir.initialize(arg); TickType_t last = xTaskGetTickCount(); @@ -205,7 +201,7 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { .az = current_accelerometer.az }; float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; - + float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; yessir.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); KalmanData current_state = yessir.getState(); From 897e1291efb198638feffb79288e0a5acb1b3c74 Mon Sep 17 00:00:00 2001 From: Shishir Date: Sat, 1 Feb 2025 22:36:08 -0600 Subject: [PATCH 14/22] fixed compilation errors :) Co-authored-by: aneeshg5 Co-authored-by: divijgarg Co-authored-by: Keshav B --- MIDAS/src/gnc/kalman_filter.h | 1 - 1 file changed, 1 deletion(-) diff --git a/MIDAS/src/gnc/kalman_filter.h b/MIDAS/src/gnc/kalman_filter.h index cef1379e..61571599 100644 --- a/MIDAS/src/gnc/kalman_filter.h +++ b/MIDAS/src/gnc/kalman_filter.h @@ -59,7 +59,6 @@ class KalmanFilter virtual void initialize(RocketSystems* args) = 0; virtual void priori() = 0; virtual void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) = 0; - virtual KalmanData getState() = 0; virtual void setState(KalmanState state) = 0; }; From b9700b186c0a222e8844222ae16919303374466f Mon Sep 17 00:00:00 2001 From: Shishir Date: Sat, 1 Feb 2025 23:02:58 -0600 Subject: [PATCH 15/22] made small fixes to systems Co-authored-by: aneeshg5 Co-authored-by: divijgarg Co-authored-by: Keshav B --- MIDAS/src/gnc/ekf.cpp | 547 ++++++++++++++++++++++++++++++++++++++++++ MIDAS/src/gnc/ekf.h | 54 +++++ MIDAS/src/systems.cpp | 10 +- 3 files changed, 606 insertions(+), 5 deletions(-) create mode 100644 MIDAS/src/gnc/ekf.cpp create mode 100644 MIDAS/src/gnc/ekf.h diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp new file mode 100644 index 00000000..fbce52e8 --- /dev/null +++ b/MIDAS/src/gnc/ekf.cpp @@ -0,0 +1,547 @@ +#include "ekf.h" +#include "finite-state-machines/fsm_states.h" + +EKF::EKF() : KalmanFilter() { + state = KalmanData(); + initializeAerodynamicData(); +} + +/** + * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms + * apart + * + * The following for loop takes a series of barometer measurements on start + * up and takes the average of them in order to initialize the kalman filter + * to the correct initial barometric altitude. This is done so that the + * kalman filter takes minimal time to converge to an accurate state + * estimate. This process is significantly faster than allowing the state as + * letting the filter to converge to the correct state can take up to 3 min. + * This specific process was used because the barometric altitude will + * change depending on the weather and thus, the initial state estimate + * cannot be hard coded. A GPS altitude may be used instead but due to GPS + * losses during high speed/high altitude flight, it is inadvisable with the + * current hardware to use this as a solution. Reference frames should also + * be kept consistent (do not mix GPS altitude and barometric). + * + */ + +const float pi = 3.14159268; +const float rho = 1.225; +const float r = 0.03935;//meters +const float height_full = 2.34; //height of rocket Full Stage - 2.34 m +const float height_sustainer = 1.34; +const float mass_full = 7.57; //kg Sustainer + Booster +const float mass_sustainer = 4.08; //kg Sustainer +// Moonburner motor +const std::map moonburner_data = { + {0.083, 1333.469}, + {0.13, 1368.376}, + {0.249, 1361.395}, + {0.308, 1380.012}, + {0.403, 1359.068}, + {0.675, 1184.53}, + {1.018, 1072.826}, + {1.456, 996.029}, + {1.977, 958.794}, + {2.995, 914.578}, + {3.99, 856.399}, + {4.985, 781.929}, + {5.494, 730.732}, + {5.991, 679.534}, + {7.258, 542.231}, + {7.862, 463.107}, + {8.015, 456.125}, + {8.998, 330.458}, + {9.993, 207.118}, + {10.514, 137.303}, + {11.496, 34.908}, + {11.994, 0.0} +}; + +// Booster motor +std::map O5500X_data = { + {0.009, 20.408}, + {0.044, 7112.245}, + {0.063, 6734.694}, + {0.078, 6897.959}, + {0.094, 6612.245}, + {0.109, 6765.306}, + {0.125, 6540.816}, + {0.147, 6581.633}, + {0.194, 6520.408}, + {0.35, 6795.918}, + {0.428, 7091.837}, + {0.563, 7285.714}, + {0.694, 7408.163}, + {0.988, 7581.633}, + {1.266, 7622.449}, + {1.491, 7724.49}, + {1.581, 7653.061}, + {1.641, 7540.816}, + {1.684, 7500.0}, + {1.716, 7336.735}, + {1.784, 7224.49}, + {1.938, 6785.714}, + {2.138, 6326.531}, + {2.491, 5897.959}, + {2.6, 5704.082}, + {2.919, 3540.816}, + {3.022, 3408.163}, + {3.138, 2887.755}, + {3.3, 2234.694}, + {3.388, 1673.469}, + {3.441, 1489.796}, + {3.544, 1418.367}, + {3.609, 1295.918}, + {3.688, 816.327}, + {3.778, 653.061}, + {3.819, 581.633}, + {3.853, 489.796}, + {3.897, 285.714}, + {3.981, 20.408}, + {3.997, 0.0} +}; + + +void EKF::initialize(RocketSystems* args) { + Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); + float sum = 0; + + for (int i = 0; i < 30; i++) { + Barometer barometer = args->rocket_data.barometer.getRecent(); + LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); + Acceleration accelerations = { + .ax = initial_accelerometer.ax, + .ay = initial_accelerometer.ay, + .az = initial_accelerometer.az + }; + sum += barometer.altitude; + + init_accel(0, 0) += accelerations.az; + init_accel(1, 0) += accelerations.ay; + init_accel(2, 0) += -accelerations.ax; + THREAD_SLEEP(100); + } + + init_accel(0, 0) /= 30; + init_accel(1, 0) /= 30; + init_accel(2, 0) /= 30; + + euler_t euler = orientation.getEuler(); + euler.yaw = -euler.yaw; + world_accel = BodyToGlobal(euler, init_accel); + + // set x_k + x_k(0, 0) = sum / 30; + x_k(3, 0) = 0; + x_k(6, 0) = 0; + + F_mat.setZero(); // Initialize with zeros + + Q(0, 0) = pow(s_dt, 5) / 20; + Q(0, 1) = pow(s_dt, 4) / 8; + Q(0, 2) = pow(s_dt, 3) / 6; + Q(1, 1) = pow(s_dt, 3) / 8; + Q(1, 2) = pow(s_dt, 2) / 2; + Q(2, 2) = s_dt; + Q(1, 0) = Q(0, 1); + Q(2, 0) = Q(0, 2); + Q(2, 1) = Q(1, 2); + + Q(3, 3) = pow(s_dt, 5) / 20; + Q(3, 4) = pow(s_dt, 4) / 8; + Q(3, 5) = pow(s_dt, 3) / 6; + Q(4, 4) = pow(s_dt, 3) / 8; + Q(4, 5) = pow(s_dt, 2) / 2; + Q(5, 5) = s_dt; + Q(4, 3) = Q(3, 4); + Q(5, 3) = Q(3, 5); + Q(5, 4) = Q(4, 5); + + Q(6, 6) = pow(s_dt, 5) / 20; + Q(6, 7) = pow(s_dt, 4) / 8; + Q(6, 8) = pow(s_dt, 3) / 6; + Q(7, 7) = pow(s_dt, 3) / 8; + Q(7, 8) = pow(s_dt, 2) / 2; + Q(8, 8) = s_dt; + Q(7, 6) = Q(6, 7); + Q(8, 6) = Q(6, 8); + Q(8, 7) = Q(7, 8); + + // set H + H(0, 0) = 1; + H(1, 2) = 1; + H(2, 5) = 1; + H(3, 8) = 1; + + Q = Q * spectral_density_; + + // set R + R(0, 0) = 2.0; + R(1, 1) = 1.9; + R(2, 2) = 10; + R(3, 3) = 10; + + // set B (don't care about what's in B since we have no control input) + B(2, 0) = -1; + +} + + +void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { + Eigen::Matrix xdot = Eigen::Matrix::Zero(); + + Velocity omega = orientation.getVelocity(); + euler_t angles = orientation.getEuler(); + Eigen::Matrix gravity = Eigen::Matrix::Zero(); + gravity(0, 0) = -9.81; + Eigen::Matrix ang_pos = BodyToGlobal(angles, init_accel) + gravity; + + float m = mass_sustainer; + float h = height_sustainer; + if (fsm < FSMState::STATE_BURNOUT) { + m = mass_full; + h= height_full; + } + float w_x = omega.vx; + float w_y = omega.vy; + float w_z = omega.vz; + + float J_x = 0.5 * m * r * r; + float J_y = (1/3) * m * h * h + 0.25 * m * r * r; + float J_z = J_y; + + float vel_mag_squared = x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0); + + float Fax = -0.5*rho*(vel_mag_squared)*float(Ca)*(pi*r*r); + float Fay = 0; + float Faz = 0; + + Eigen::Matrix Fg_body = GlobalToBody(angles, gravity); + float Fgx = Fg_body(0,0); float Fgy = Fg_body(1,0); float Fgz = Fg_body(2,0); + + Eigen::Matrix T = EKF::getThrust(stage_timestamp, angles, fsm); + float Ftx = T(0, 0); float Fty = T(1, 0); float Ftz = T(2, 0); + + + xdot << x_k(1,0), + (Fax + Ftx + Fgx) / m - (w_y * x_k(7,0) - w_z * x_k(4,0)), + 1.0, + + x_k(4,0), + (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), // this might not be right + 1.0, + + x_k(7,0), + (Faz + Ftz + Fgz) / m - (w_x * x_k(4,0) - w_y * x_k(1,0)), + 1.0; + + x_priori = (xdot * dt) + x_k; + setF(dt, fsm, w_x, w_y, w_z); + P_priori = (F_mat * P_k * F_mat.transpose()) + Q; +} + + +float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) { + return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); +} + + +Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { + float interpolatedValue = 0; + if (FSM_state >= STATE_FIRST_BOOST){ + if (FSM_state < FSMState::STATE_BURNOUT ) { + // interpolate from O5500X_data + if(timestamp >=0.009){ + auto it = O5500X_data.lower_bound(timestamp); + if (it != O5500X_data.end()) { + float x0 = it->first; + float y0 = it->second; + ++it; + float x1 = it->first; + float y1 = it->second; + interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); + } + } + } + else { + // interpolate from moonburner_data + if(timestamp >=0.083){ + auto it = moonburner_data.lower_bound(timestamp); + if (it != moonburner_data.end()) { + float x0 = it->first; + float y0 = it->second; + ++it; + float x1 = it->first; + float y1 = it->second; + interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); + } + } + } + } + Eigen::Matrix interpolatedVector; + interpolatedVector(0,0) = interpolatedValue; + return BodyToGlobal(angles, interpolatedVector); +} + +/** + * @brief Update Kalman Gain and state estimate with current sensor data + * + * After receiving new sensor data, the Kalman filter updates the state estimate + * and Kalman gain. The Kalman gain can be considered as a measure of how uncertain + * the new sensor data is. After updating the gain, the state estimate is updated. + * + */ +void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { + if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { + float sum = 0; + float data[10]; + alt_buffer.readSlice(data, 0, 10); + for (float i : data) { + sum += i; + } + KalmanState kalman_state = (KalmanState){sum / 10.0f, 0, 0, 0, 0, 0, 0, 0, 0}; + setState(kalman_state); + } else if (FSM_state >= FSMState::STATE_APOGEE) { + H(1, 2) = 0; + } + + Eigen::Matrix S_k = Eigen::Matrix::Zero(); + S_k = (((H * P_priori * H.transpose()) + R)).inverse(); + Eigen::Matrix identity = Eigen::Matrix::Identity(); + K = (P_priori * H.transpose()) * S_k; + + // Sensor Measurements + Eigen::Matrix accel = Eigen::Matrix::Zero(); + + accel(0, 0) = acceleration.az - 0.045; + accel(1, 0) = acceleration.ay - 0.065; + accel(2, 0) = -acceleration.ax - 0.06; + + euler_t angles = orientation.getEuler(); + angles.yaw = -angles.yaw; + + Eigen::Matrix acc = BodyToGlobal(angles, accel); + + y_k(1, 0) = (acc(0)) * 9.81 - 9.81; + y_k(2, 0) = (acc(1)) * 9.81; + y_k(3, 0) = (acc(2)) * 9.81; + + y_k(0, 0) = barometer.altitude; + alt_buffer.push(barometer.altitude); + + + // # Posteriori Update + x_k = x_priori + K * (y_k - (H * x_priori)); + P_k = (identity - K * H) * P_priori; + // Joseph (Expanded) Form + // P_k = (identity - K * H) * P_priori * (identity - K * H).transpose() + K * R * K.transpose(); + + kalman_state.state_est_pos_x = x_k(0, 0); + kalman_state.state_est_vel_x = x_k(1, 0); + kalman_state.state_est_accel_x = x_k(2, 0); + kalman_state.state_est_pos_y = x_k(3, 0); + kalman_state.state_est_vel_y = x_k(4, 0); + kalman_state.state_est_accel_y = x_k(5, 0); + kalman_state.state_est_pos_z = x_k(6, 0); + kalman_state.state_est_vel_z = x_k(7, 0); + kalman_state.state_est_accel_z = x_k(8, 0); + + state.position = (Position){kalman_state.state_est_pos_x,kalman_state.state_est_pos_y,kalman_state.state_est_pos_z}; + state.velocity = (Velocity){kalman_state.state_est_vel_x,kalman_state.state_est_vel_y,kalman_state.state_est_vel_z}; + state.acceleration = (Acceleration){kalman_state.state_est_accel_x,kalman_state.state_est_accel_y,kalman_state.state_est_accel_z}; +} + +/** + * @brief Run Kalman filter calculations as long as FSM has passed IDLE + * + * @param dt Time step calculated by the Kalman Filter Thread + * @param sd Spectral density of the noise + * @param &barometer Data of the current barometer + * @param acceleration Current acceleration + * @param &orientation Current orientation + * @param current_state Current FSM_state + */ +void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) { + if (FSM_state >= FSMState::STATE_IDLE) { + if (FSM_state != last_fsm) { + stage_timestamp = 0; + } + stage_timestamp += dt; + setF(dt, FSM_state, orientation.roll, orientation.pitch, orientation.yaw); + setQ(dt, sd); + priori(dt, orientation, FSM_state); + update(barometer, acceleration, orientation, FSM_state); + FSMState last_fsm = FSM_state; + } +} +//LookUpMap + +void EKF::initializeAerodynamicData() { + std::ifstream file("LookUp.csv"); + std::string line; + + // Skip the header line + std::getline(file, line); + + while (std::getline(file, line)) { + std::istringstream iss(line); + std::string token; + std::vector row; + + while (std::getline(iss, token, ',')) { + row.push_back(std::stod(token)); + } + + if (row.size() >= 5) { + double mach = std::round(row[0] / 0.04) * 0.04; + aerodynamicData[mach] = std::make_tuple(row[2], row[3], row[4]); + } + } +} + +/** + * @brief Getter for state X + * + * @return the current state, see sensor_data.h for KalmanData + */ +KalmanData EKF::getState() { + return state; +} + +/** + * @brief Sets state vector x + * + * @param state Wanted state vector + */ +void EKF::setState(KalmanState state) { + this->state.position.px = state.state_est_pos_x; + this->state.position.py = state.state_est_pos_y; + this->state.position.pz = state.state_est_pos_z; + this->state.acceleration.ax = state.state_est_accel_x; + this->state.acceleration.ay = state.state_est_accel_y; + this->state.acceleration.az = state.state_est_accel_z; + this->state.velocity.vx = state.state_est_vel_x; + this->state.velocity.vy = state.state_est_vel_y; + this->state.velocity.vz = state.state_est_vel_z; +} + +/** + * @brief Sets the Q matrix given time step and spectral density. + * + * @param dt Time step calculated by the Kalman Filter Thread + * @param sd Spectral density of the noise + * + * The Q matrix is the covariance matrix for the process noise and is + * updated based on the time taken per cycle of the Kalman Filter Thread. + */ +void EKF::setQ(float dt, float sd) { + Q(0, 0) = pow(dt, 5) / 20; + Q(0, 1) = pow(dt, 4) / 8; + Q(0, 2) = pow(dt, 3) / 6; + Q(1, 1) = pow(dt, 3) / 8; + Q(1, 2) = pow(dt, 2) / 2; + Q(2, 2) = dt; + Q(1, 0) = Q(0, 1); + Q(2, 0) = Q(0, 2); + Q(2, 1) = Q(1, 2); + Q(3, 3) = pow(dt, 5) / 20; + Q(3, 4) = pow(dt, 4) / 8; + Q(3, 5) = pow(dt, 3) / 6; + Q(4, 4) = pow(dt, 3) / 8; + Q(4, 5) = pow(dt, 2) / 2; + Q(5, 5) = dt; + Q(4, 3) = Q(3, 4); + Q(5, 3) = Q(3, 5); + Q(5, 4) = Q(4, 5); + + Q(6, 6) = pow(dt, 5) / 20; + Q(6, 7) = pow(dt, 4) / 8; + Q(6, 8) = pow(dt, 3) / 6; + Q(7, 7) = pow(dt, 3) / 8; + Q(7, 8) = pow(dt, 2) / 2; + Q(8, 8) = dt; + Q(7, 6) = Q(6, 7); + Q(8, 6) = Q(6, 8); + Q(8, 7) = Q(7, 8); + + Q *= sd; +} + +/** + * @brief Converts a vector in the body frame to the global frame + * + * @param angles Roll, pitch, yaw angles + * @param body_vect Vector for rotation in the body frame + * @return Eigen::Matrix Rotated vector in the global frame + */ +Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { + Eigen::Matrix3f roll, pitch, yaw; + roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); + pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); + yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; + return yaw * pitch * roll * body_vect; +} + +void EKF::priori(){}; + +Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { + //pysim code + // roll = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) + // pitch = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) + // yaw = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) + // return (yaw @ pitch @ roll).T @ world_vector + Eigen::Matrix3f roll; + roll << 1, 0, 0, 0, cos(angles.roll), -sin(angles.roll), 0, sin(angles.roll), cos(angles.roll); + Eigen::Matrix3f pitch; + pitch << cos(angles.pitch), 0, sin(angles.pitch), 0, 1, 0,-sin(angles.pitch), 0, cos(angles.pitch); + Eigen::Matrix3f yaw; + yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1; + Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; + return rotation_matrix.transpose() * world_vector; +} + +/** + * @brief Sets the F matrix given time step. + * + * @param dt Time step calculated by the Kalman Filter Thread + * + * The F matrix is the state transition matrix and is defined + * by how the states change over time. + */ +void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { + Eigen::Matrix w = Eigen::Matrix::Zero(); + w(0, 0) = wx; + w(1, 0) = wy; + w(2, 0) = wz; + F_mat(0,1) = 1; + F_mat(3,4) = 1; + F_mat(6,7) = 1; + + float m = mass_sustainer; + float h = height_sustainer; + if (fsm < FSMState::STATE_BURNOUT) { + m = mass_full; + h= height_full; + } + + F_mat(1,1) = -pi * Ca * r * r * rho * x_k(1,0) / m; + F_mat(1,4) = -pi * Ca * r * r * rho * x_k(4,0) / m + w(2,0); + F_mat(1,7) = -pi * Ca * r * r * rho * x_k(7,0) / m - w(1,0); + + F_mat(4,1) = pi * Cn * r * r * rho * x_k(1,0) / m - w(2,0); + F_mat(4,4) = pi * Cn * r * r * rho * x_k(2,0) / m + w(0,0); + F_mat(4,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + + F_mat(7,1) = pi * Cn * r * r * rho * x_k(1,0) / m + w(1,0); + F_mat(7,4) = pi * Cn * r * r * rho * x_k(2,0) / m - w(0,0); + F_mat(7,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + + float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); + float mach = velocity_magnitude / 343.0; + mach = std::round(mach / 0.04) * 0.04; + std::tie(Ca, Cn, Cp) = aerodynamicData[mach]; + +} + +EKF ekf; diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h new file mode 100644 index 00000000..667851ec --- /dev/null +++ b/MIDAS/src/gnc/ekf.h @@ -0,0 +1,54 @@ +#pragma once + +#include "kalman_filter.h" +#include "sensor_data.h" +#include "Buffer.h" +#include + +#define NUM_STATES 9 +#define NUM_SENSOR_INPUTS 4 +#define ALTITUDE_BUFFER_SIZE 10 + +class EKF : public KalmanFilter +{ +public: + EKF(); + void initialize(RocketSystems* args) override; + void priori(); + void priori(float dt, Orientation &orientation, FSMState fsm); + void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override; + + void setQ(float dt, float sd); + void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z); + Eigen::Matrix BodyToGlobal(euler_t angles, Eigen::Matrix x_k); + Eigen::Matrix getThrust(float timestamp, euler_t angles, FSMState FSM_state); + Eigen::Matrix GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); + + KalmanData getState() override; + void setState(KalmanState state) override; + + void initializeAerodynamicData(); + float linearInterpolation(float x0, float y0, float x1, float y1, float x); + + void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state); + + bool should_reinit = false; +private: + std::map> aerodynamicData; + float s_dt = 0.05f; + float spectral_density_ = 13.0f; + float kalman_apo = 0; + float Ca = 0; + float Cn = 0; + float Cp = 0; + KalmanState kalman_state; + FSMState last_fsm = FSMState::STATE_IDLE; + float stage_timestamp = 0; + + Eigen::Matrix init_accel = Eigen::Matrix::Zero(); + Eigen::Matrix world_accel; + Buffer alt_buffer; + KalmanData state; +}; + +extern EKF ekf; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index a4a8f44a..4ca59d68 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -1,7 +1,7 @@ #include "systems.h" #include "hal.h" -#include "gnc/yessir.h" +#include "gnc/ekf.h" #include @@ -181,12 +181,12 @@ DECLARE_THREAD(buzzer, RocketSystems* arg) { } DECLARE_THREAD(kalman, RocketSystems* arg) { - yessir.initialize(arg); + ekf.initialize(arg); TickType_t last = xTaskGetTickCount(); while (true) { if(arg->rocket_data.command_flags.should_reset_kf){ - yessir.initialize(arg); + ekf.initialize(arg); TickType_t last = xTaskGetTickCount(); arg->rocket_data.command_flags.should_reset_kf = false; } @@ -202,8 +202,8 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { }; float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; - yessir.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); - KalmanData current_state = yessir.getState(); + ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); + KalmanData current_state = ekf.getState(); arg->rocket_data.kalman.update(current_state); From 4c9661cc3da97a6c775e027e4c4e437381fd848d Mon Sep 17 00:00:00 2001 From: Shishir Date: Sun, 2 Feb 2025 14:23:00 -0600 Subject: [PATCH 16/22] documentation --- MIDAS/src/gnc/ekf.cpp | 126 +++++++++++++++++++++++++++--------------- 1 file changed, 80 insertions(+), 46 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index fbce52e8..dfbdd3a8 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -6,33 +6,16 @@ EKF::EKF() : KalmanFilter() { initializeAerodynamicData(); } -/** - * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms - * apart - * - * The following for loop takes a series of barometer measurements on start - * up and takes the average of them in order to initialize the kalman filter - * to the correct initial barometric altitude. This is done so that the - * kalman filter takes minimal time to converge to an accurate state - * estimate. This process is significantly faster than allowing the state as - * letting the filter to converge to the correct state can take up to 3 min. - * This specific process was used because the barometric altitude will - * change depending on the weather and thus, the initial state estimate - * cannot be hard coded. A GPS altitude may be used instead but due to GPS - * losses during high speed/high altitude flight, it is inadvisable with the - * current hardware to use this as a solution. Reference frames should also - * be kept consistent (do not mix GPS altitude and barometric). - * - */ - +// constants const float pi = 3.14159268; -const float rho = 1.225; -const float r = 0.03935;//meters -const float height_full = 2.34; //height of rocket Full Stage - 2.34 m +const float rho = 1.225; // average air density +const float r = 0.03935; // meters +const float height_full = 2.34; // height of rocket Full Stage - 2.34 m const float height_sustainer = 1.34; -const float mass_full = 7.57; //kg Sustainer + Booster -const float mass_sustainer = 4.08; //kg Sustainer -// Moonburner motor +const float mass_full = 7.57; // kg Sustainer + Booster +const float mass_sustainer = 4.08; // kg Sustainer + +// Moonburner motor thrust curve (Sustainer) const std::map moonburner_data = { {0.083, 1333.469}, {0.13, 1368.376}, @@ -58,7 +41,7 @@ const std::map moonburner_data = { {11.994, 0.0} }; -// Booster motor +// O5500X motor thrust curve (Booster) std::map O5500X_data = { {0.009, 20.408}, {0.044, 7112.245}, @@ -102,6 +85,24 @@ std::map O5500X_data = { {3.997, 0.0} }; +/** + * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms + * apart + * + * The following for loop takes a series of barometer measurements on start + * up and takes the average of them in order to initialize the kalman filter + * to the correct initial barometric altitude. This is done so that the + * kalman filter takes minimal time to converge to an accurate state + * estimate. This process is significantly faster than allowing the state as + * letting the filter to converge to the correct state can take up to 3 min. + * This specific process was used because the barometric altitude will + * change depending on the weather and thus, the initial state estimate + * cannot be hard coded. A GPS altitude may be used instead but due to GPS + * losses during high speed/high altitude flight, it is inadvisable with the + * current hardware to use this as a solution. Reference frames should also + * be kept consistent (do not mix GPS altitude and barometric). + * + */ void EKF::initialize(RocketSystems* args) { Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); @@ -187,6 +188,13 @@ void EKF::initialize(RocketSystems* args) { } +/** + * @brief Estimates current state of the rocket without current sensor data + * + * The priori step of the Kalman filter is used to estimate the current state + * of the rocket without knowledge of the current sensor data. In other words, + * it extrapolates the state at time n+1 based on the state at time n. + */ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { Eigen::Matrix xdot = Eigen::Matrix::Zero(); @@ -197,15 +205,11 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { gravity(0, 0) = -9.81; Eigen::Matrix ang_pos = BodyToGlobal(angles, init_accel) + gravity; - float m = mass_sustainer; - float h = height_sustainer; + float m = mass_sustainer; float h = height_sustainer; if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; - h= height_full; + m = mass_full; h = height_full; } - float w_x = omega.vx; - float w_y = omega.vy; - float w_z = omega.vz; + float w_x = omega.vx; float w_y = omega.vy; float w_z = omega.vz; float J_x = 0.5 * m * r * r; float J_y = (1/3) * m * h * h + 0.25 * m * r * r; @@ -214,8 +218,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { float vel_mag_squared = x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0); float Fax = -0.5*rho*(vel_mag_squared)*float(Ca)*(pi*r*r); - float Fay = 0; - float Faz = 0; + float Fay = 0; float Faz = 0; Eigen::Matrix Fg_body = GlobalToBody(angles, gravity); float Fgx = Fg_body(0,0); float Fgy = Fg_body(1,0); float Fgz = Fg_body(2,0); @@ -229,7 +232,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { 1.0, x_k(4,0), - (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), // this might not be right + (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), 1.0, x_k(7,0), @@ -241,18 +244,33 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } - +/** + * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim + */ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) { return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); } - +/** + * @brief Returns the approximate thrust force from the motor given the thurst curve + * + * @param timestamp Time since most recent ignition + * @param angles Current orientation of the rocket + * @param FSM_state Current FSM state + * + * @return Thrust force in the body frame + * + * The thrust force is calculated by interpolating the thrust curve data which is stored in an ordered map (see top of file). + * The thrust curve data is different for the booster and sustainer stages, so the function checks the FSM state to determine + * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. + * The thrust force is then rotated into the body frame using the BodyToGlobal function. + */ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { float interpolatedValue = 0; if (FSM_state >= STATE_FIRST_BOOST){ if (FSM_state < FSMState::STATE_BURNOUT ) { - // interpolate from O5500X_data - if(timestamp >=0.009){ + // first stage + if(timestamp >= 0.009){ auto it = O5500X_data.lower_bound(timestamp); if (it != O5500X_data.end()) { float x0 = it->first; @@ -265,8 +283,8 @@ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMSt } } else { - // interpolate from moonburner_data - if(timestamp >=0.083){ + if (timestamp >= 0.083){ + // second stage auto it = moonburner_data.lower_bound(timestamp); if (it != moonburner_data.end()) { float x0 = it->first; @@ -334,8 +352,6 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); P_k = (identity - K * H) * P_priori; - // Joseph (Expanded) Form - // P_k = (identity - K * H) * P_priori * (identity - K * H).transpose() + K * R * K.transpose(); kalman_state.state_est_pos_x = x_k(0, 0); kalman_state.state_est_vel_x = x_k(1, 0); @@ -375,8 +391,13 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati FSMState last_fsm = FSM_state; } } -//LookUpMap +/** + * @brief Initializes the aerodynamic data from a CSV file + * + * The function reads a CSV file containing aerodynamic data and stores it in a map. + * The key is the Mach number, and the value is a tuple containing aerodynamic coefficients. + */ void EKF::initializeAerodynamicData() { std::ifstream file("LookUp.csv"); std::string line; @@ -483,8 +504,20 @@ Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix Rotated vector in the body frame + * + */ Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { //pysim code // roll = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) @@ -507,7 +540,8 @@ Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix * @param dt Time step calculated by the Kalman Filter Thread * * The F matrix is the state transition matrix and is defined - * by how the states change over time. + * by how the states change over time and also depends on the + * current state of the rocket. */ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { Eigen::Matrix w = Eigen::Matrix::Zero(); From 033b998d364a7e599aad22a4b29a6f37beaed71d Mon Sep 17 00:00:00 2001 From: Keshav B Date: Mon, 3 Feb 2025 19:50:40 -0600 Subject: [PATCH 17/22] Fixed memory allocation on ekf.cpp Co-authored-by: divijgarg --- MIDAS/src/gnc/ekf.cpp | 157 ++++++++++++++++++++++++++++-------------- MIDAS/src/gnc/ekf.h | 8 +-- 2 files changed, 111 insertions(+), 54 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index dfbdd3a8..8c66a595 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,20 +1,23 @@ #include "ekf.h" #include "finite-state-machines/fsm_states.h" + EKF::EKF() : KalmanFilter() { state = KalmanData(); initializeAerodynamicData(); } + // constants const float pi = 3.14159268; const float rho = 1.225; // average air density const float r = 0.03935; // meters -const float height_full = 2.34; // height of rocket Full Stage - 2.34 m +const float height_full = 2.34; // height of rocket Full Stage - 2.34 m const float height_sustainer = 1.34; const float mass_full = 7.57; // kg Sustainer + Booster const float mass_sustainer = 4.08; // kg Sustainer + // Moonburner motor thrust curve (Sustainer) const std::map moonburner_data = { {0.083, 1333.469}, @@ -41,6 +44,7 @@ const std::map moonburner_data = { {11.994, 0.0} }; + // O5500X motor thrust curve (Booster) std::map O5500X_data = { {0.009, 20.408}, @@ -85,6 +89,7 @@ std::map O5500X_data = { {3.997, 0.0} }; + /** * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms * apart @@ -104,10 +109,11 @@ std::map O5500X_data = { * */ + void EKF::initialize(RocketSystems* args) { Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); float sum = 0; - + for (int i = 0; i < 30; i++) { Barometer barometer = args->rocket_data.barometer.getRecent(); LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); @@ -118,27 +124,31 @@ void EKF::initialize(RocketSystems* args) { }; sum += barometer.altitude; + init_accel(0, 0) += accelerations.az; init_accel(1, 0) += accelerations.ay; init_accel(2, 0) += -accelerations.ax; THREAD_SLEEP(100); } + init_accel(0, 0) /= 30; init_accel(1, 0) /= 30; init_accel(2, 0) /= 30; + euler_t euler = orientation.getEuler(); euler.yaw = -euler.yaw; - world_accel = BodyToGlobal(euler, init_accel); // set x_k x_k(0, 0) = sum / 30; x_k(3, 0) = 0; x_k(6, 0) = 0; + F_mat.setZero(); // Initialize with zeros + Q(0, 0) = pow(s_dt, 5) / 20; Q(0, 1) = pow(s_dt, 4) / 8; Q(0, 2) = pow(s_dt, 3) / 6; @@ -149,6 +159,7 @@ void EKF::initialize(RocketSystems* args) { Q(2, 0) = Q(0, 2); Q(2, 1) = Q(1, 2); + Q(3, 3) = pow(s_dt, 5) / 20; Q(3, 4) = pow(s_dt, 4) / 8; Q(3, 5) = pow(s_dt, 3) / 6; @@ -159,6 +170,7 @@ void EKF::initialize(RocketSystems* args) { Q(5, 3) = Q(3, 5); Q(5, 4) = Q(4, 5); + Q(6, 6) = pow(s_dt, 5) / 20; Q(6, 7) = pow(s_dt, 4) / 8; Q(6, 8) = pow(s_dt, 3) / 6; @@ -169,25 +181,31 @@ void EKF::initialize(RocketSystems* args) { Q(8, 6) = Q(6, 8); Q(8, 7) = Q(7, 8); + // set H H(0, 0) = 1; H(1, 2) = 1; H(2, 5) = 1; H(3, 8) = 1; + Q = Q * spectral_density_; + // set R R(0, 0) = 2.0; R(1, 1) = 1.9; R(2, 2) = 10; R(3, 3) = 10; + // set B (don't care about what's in B since we have no control input) B(2, 0) = -1; + } + /** * @brief Estimates current state of the rocket without current sensor data * @@ -196,54 +214,62 @@ void EKF::initialize(RocketSystems* args) { * it extrapolates the state at time n+1 based on the state at time n. */ + void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { Eigen::Matrix xdot = Eigen::Matrix::Zero(); - Velocity omega = orientation.getVelocity(); euler_t angles = orientation.getEuler(); - Eigen::Matrix gravity = Eigen::Matrix::Zero(); - gravity(0, 0) = -9.81; - Eigen::Matrix ang_pos = BodyToGlobal(angles, init_accel) + gravity; - + // Eigen::Matrix gravity = Eigen::Matrix::Zero(); + gravity(0, 0) = -9.81; float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) { + if (fsm < FSMState::STATE_BURNOUT) { m = mass_full; h = height_full; } float w_x = omega.vx; float w_y = omega.vy; float w_z = omega.vz; - + float J_x = 0.5 * m * r * r; float J_y = (1/3) * m * h * h + 0.25 * m * r * r; float J_z = J_y; float vel_mag_squared = x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0); + float Fax = -0.5*rho*(vel_mag_squared)*float(Ca)*(pi*r*r); float Fay = 0; float Faz = 0; - Eigen::Matrix Fg_body = GlobalToBody(angles, gravity); - float Fgx = Fg_body(0,0); float Fgy = Fg_body(1,0); float Fgz = Fg_body(2,0); - Eigen::Matrix T = EKF::getThrust(stage_timestamp, angles, fsm); - float Ftx = T(0, 0); float Fty = T(1, 0); float Ftz = T(2, 0); + Eigen::Matrix *Fg_body = (GlobalToBody(angles, gravity)); + float Fgx = (*Fg_body)(0,0); float Fgy = (*Fg_body)(1,0); float Fgz = (*Fg_body)(2,0); - + delete Fg_body; + + Eigen::Matrix * T = EKF::getThrust(stage_timestamp, angles, fsm); + float Ftx = (*T)(0, 0); float Fty = (*T)(1, 0); float Ftz = (*T)(2, 0); + + delete T; + + xdot << x_k(1,0), (Fax + Ftx + Fgx) / m - (w_y * x_k(7,0) - w_z * x_k(4,0)), 1.0, + x_k(4,0), (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), 1.0, + x_k(7,0), (Faz + Ftz + Fgz) / m - (w_x * x_k(4,0) - w_y * x_k(1,0)), 1.0; + x_priori = (xdot * dt) + x_k; setF(dt, fsm, w_x, w_y, w_z); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } + /** * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim */ @@ -251,21 +277,22 @@ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); } + /** * @brief Returns the approximate thrust force from the motor given the thurst curve - * + * * @param timestamp Time since most recent ignition * @param angles Current orientation of the rocket * @param FSM_state Current FSM state - * + * * @return Thrust force in the body frame - * + * * The thrust force is calculated by interpolating the thrust curve data which is stored in an ordered map (see top of file). - * The thrust curve data is different for the booster and sustainer stages, so the function checks the FSM state to determine - * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. + * The thrust curve data is different for the booster and sustainer stages, so the function checks the FSM state to determine + * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. * The thrust force is then rotated into the body frame using the BodyToGlobal function. */ -Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { +Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { float interpolatedValue = 0; if (FSM_state >= STATE_FIRST_BOOST){ if (FSM_state < FSMState::STATE_BURNOUT ) { @@ -278,10 +305,10 @@ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMSt ++it; float x1 = it->first; float y1 = it->second; - interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); + interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); } } - } + } else { if (timestamp >= 0.083){ // second stage @@ -302,6 +329,7 @@ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMSt return BodyToGlobal(angles, interpolatedVector); } + /** * @brief Update Kalman Gain and state estimate with current sensor data * @@ -311,7 +339,7 @@ Eigen::Matrix EKF::getThrust(float timestamp, euler_t angles, FSMSt * */ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { - if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { + if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { float sum = 0; float data[10]; alt_buffer.readSlice(data, 0, 10); @@ -324,35 +352,44 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori H(1, 2) = 0; } + Eigen::Matrix S_k = Eigen::Matrix::Zero(); S_k = (((H * P_priori * H.transpose()) + R)).inverse(); Eigen::Matrix identity = Eigen::Matrix::Identity(); K = (P_priori * H.transpose()) * S_k; + // Sensor Measurements Eigen::Matrix accel = Eigen::Matrix::Zero(); - + accel(0, 0) = acceleration.az - 0.045; accel(1, 0) = acceleration.ay - 0.065; accel(2, 0) = -acceleration.ax - 0.06; + euler_t angles = orientation.getEuler(); angles.yaw = -angles.yaw; - Eigen::Matrix acc = BodyToGlobal(angles, accel); - y_k(1, 0) = (acc(0)) * 9.81 - 9.81; - y_k(2, 0) = (acc(1)) * 9.81; - y_k(3, 0) = (acc(2)) * 9.81; + Eigen::Matrix * acc = (BodyToGlobal(angles, accel)); + + + y_k(1, 0) = ((*acc)(0)) * 9.81 - 9.81; + y_k(2, 0) = ((*acc)(1)) * 9.81; + y_k(3, 0) = ((*acc)(2)) * 9.81; + + delete acc; y_k(0, 0) = barometer.altitude; alt_buffer.push(barometer.altitude); + + // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); P_k = (identity - K * H) * P_priori; - + kalman_state.state_est_pos_x = x_k(0, 0); kalman_state.state_est_vel_x = x_k(1, 0); kalman_state.state_est_accel_x = x_k(2, 0); @@ -363,11 +400,13 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori kalman_state.state_est_vel_z = x_k(7, 0); kalman_state.state_est_accel_z = x_k(8, 0); + state.position = (Position){kalman_state.state_est_pos_x,kalman_state.state_est_pos_y,kalman_state.state_est_pos_z}; state.velocity = (Velocity){kalman_state.state_est_vel_x,kalman_state.state_est_vel_y,kalman_state.state_est_vel_z}; state.acceleration = (Acceleration){kalman_state.state_est_accel_x,kalman_state.state_est_accel_y,kalman_state.state_est_accel_z}; } + /** * @brief Run Kalman filter calculations as long as FSM has passed IDLE * @@ -392,6 +431,7 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati } } + /** * @brief Initializes the aerodynamic data from a CSV file * @@ -401,19 +441,19 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati void EKF::initializeAerodynamicData() { std::ifstream file("LookUp.csv"); std::string line; - + // Skip the header line std::getline(file, line); - + while (std::getline(file, line)) { std::istringstream iss(line); std::string token; std::vector row; - + while (std::getline(iss, token, ',')) { row.push_back(std::stod(token)); } - + if (row.size() >= 5) { double mach = std::round(row[0] / 0.04) * 0.04; aerodynamicData[mach] = std::make_tuple(row[2], row[3], row[4]); @@ -421,6 +461,7 @@ void EKF::initializeAerodynamicData() { } } + /** * @brief Getter for state X * @@ -430,6 +471,7 @@ KalmanData EKF::getState() { return state; } + /** * @brief Sets state vector x * @@ -447,6 +489,7 @@ void EKF::setState(KalmanState state) { this->state.velocity.vz = state.state_est_vel_z; } + /** * @brief Sets the Q matrix given time step and spectral density. * @@ -476,6 +519,7 @@ void EKF::setQ(float dt, float sd) { Q(5, 3) = Q(3, 5); Q(5, 4) = Q(4, 5); + Q(6, 6) = pow(dt, 5) / 20; Q(6, 7) = pow(dt, 4) / 8; Q(6, 8) = pow(dt, 3) / 6; @@ -486,9 +530,11 @@ void EKF::setQ(float dt, float sd) { Q(8, 6) = Q(6, 8); Q(8, 7) = Q(7, 8); + Q *= sd; } + /** * @brief Converts a vector in the body frame to the global frame * @@ -496,34 +542,34 @@ void EKF::setQ(float dt, float sd) { * @param body_vect Vector for rotation in the body frame * @return Eigen::Matrix Rotated vector in the global frame */ -Eigen::Matrix EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { +Eigen::Matrix * EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { Eigen::Matrix3f roll, pitch, yaw; roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; - return yaw * pitch * roll * body_vect; + + Eigen::Matrix * global = new Eigen::Matrix(yaw * pitch * roll * body_vect); + return global; } + /** * THIS IS A PLACEHOLDER FUNCTION SO WE CAN ABSTRACT FROM `kalman_filter.h` */ void EKF::priori(){}; + /** * @brief Converts a vector in the global frame to the body frame - * + * * @param angles Roll, pitch, yaw angles * @param world_vector Vector for rotation in the global frame - * + * * @return Eigen::Matrix Rotated vector in the body frame - * + * */ -Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { - //pysim code - // roll = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) - // pitch = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) - // yaw = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) - // return (yaw @ pitch @ roll).T @ world_vector +Eigen::Matrix * EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { + Eigen::Matrix3f roll; roll << 1, 0, 0, 0, cos(angles.roll), -sin(angles.roll), 0, sin(angles.roll), cos(angles.roll); Eigen::Matrix3f pitch; @@ -531,7 +577,10 @@ Eigen::Matrix EKF::GlobalToBody(euler_t angles, const Eigen::Matrix Eigen::Matrix3f yaw; yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1; Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; - return rotation_matrix.transpose() * world_vector; + (rotation_matrix).transpose(); + Eigen::Matrix * to_return = new Eigen::Matrix(rotation_matrix*world_vector); + return to_return; + } /** @@ -550,32 +599,40 @@ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { w(2, 0) = wz; F_mat(0,1) = 1; F_mat(3,4) = 1; - F_mat(6,7) = 1; + F_mat(6,7) = 1; + float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; + if (fsm < FSMState::STATE_BURNOUT) { + m = mass_full; h= height_full; } + F_mat(1,1) = -pi * Ca * r * r * rho * x_k(1,0) / m; F_mat(1,4) = -pi * Ca * r * r * rho * x_k(4,0) / m + w(2,0); F_mat(1,7) = -pi * Ca * r * r * rho * x_k(7,0) / m - w(1,0); + F_mat(4,1) = pi * Cn * r * r * rho * x_k(1,0) / m - w(2,0); F_mat(4,4) = pi * Cn * r * r * rho * x_k(2,0) / m + w(0,0); F_mat(4,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + F_mat(7,1) = pi * Cn * r * r * rho * x_k(1,0) / m + w(1,0); F_mat(7,4) = pi * Cn * r * r * rho * x_k(2,0) / m - w(0,0); F_mat(7,7) = pi * Cn * r * r * rho * x_k(3,0) / m; + float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); float mach = velocity_magnitude / 343.0; mach = std::round(mach / 0.04) * 0.04; std::tie(Ca, Cn, Cp) = aerodynamicData[mach]; - + } EKF ekf; + + + diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 667851ec..f874d6e2 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -20,9 +20,9 @@ class EKF : public KalmanFilter void setQ(float dt, float sd); void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z); - Eigen::Matrix BodyToGlobal(euler_t angles, Eigen::Matrix x_k); - Eigen::Matrix getThrust(float timestamp, euler_t angles, FSMState FSM_state); - Eigen::Matrix GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); + Eigen::Matrix *BodyToGlobal(euler_t angles, Eigen::Matrix x_k); + Eigen::Matrix *getThrust(float timestamp, euler_t angles, FSMState FSM_state); + Eigen::Matrix *GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); KalmanData getState() override; void setState(KalmanState state) override; @@ -41,12 +41,12 @@ class EKF : public KalmanFilter float Ca = 0; float Cn = 0; float Cp = 0; + Eigen::Matrix gravity = Eigen::Matrix::Zero(); KalmanState kalman_state; FSMState last_fsm = FSMState::STATE_IDLE; float stage_timestamp = 0; Eigen::Matrix init_accel = Eigen::Matrix::Zero(); - Eigen::Matrix world_accel; Buffer alt_buffer; KalmanData state; }; From d72beca65360aae58502ea72b5a7458fd7cb3dd0 Mon Sep 17 00:00:00 2001 From: Keshav B Date: Sat, 15 Feb 2025 15:30:29 -0600 Subject: [PATCH 18/22] Fixed ekf --- MIDAS/src/gnc/ekf.cpp | 375 +++++++++++++++++++++--------------------- MIDAS/src/gnc/ekf.h | 9 +- MIDAS/src/systems.cpp | 46 ++++++ 3 files changed, 235 insertions(+), 195 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 32eaf530..e42f4ca4 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,22 +1,61 @@ #include "ekf.h" #include "finite-state-machines/fsm_states.h" - -EKF::EKF() : KalmanFilter() { +EKF::EKF() : KalmanFilter() +{ state = KalmanData(); - initializeAerodynamicData(); } - // constants const float pi = 3.14159268; -const float rho = 1.225; // average air density -const float r = 0.03935; // meters -const float height_full = 2.34; // height of rocket Full Stage - 2.34 m -const float height_sustainer = 1.34; -const float mass_full = 7.57; // kg Sustainer + Booster -const float mass_sustainer = 4.08; // kg Sustainer +const float a = 343.0; // (m/s) speed of sound +const float rho = 1.225; // average air density +const float r = 0.03935; // (m) +const float height_full = 2.34; // (m) height of rocket Full Stage +const float height_sustainer = 1.34; // (m) height of rocket Sustainer +const float mass_full = 7.57; // (kg) Sustainer + Booster +const float mass_sustainer = 4.08; // (kg) Sustainer + +typedef struct +{ + float mach; + float alpha; + float CA_power_on; + float CN; + float CP; +} AeroCoeff; + +// stores the aerodynamic coefficients for the corresponding Mach number +const AeroCoeff aero_data[] = { + {0.04, 4, 1.332142905, 1.1827808455, 60.8267871968}, + {0.08, 4, 1.326587387, 1.1827808455, 60.8267871968}, + {0.12, 4, 1.31558627, 1.1827808455, 60.8267871968}, + {0.16, 4, 1.306063953, 1.1827808455, 60.8267871968}, + {0.20, 4, 1.298117084, 1.1827808455, 60.8267871968}, + {0.24, 4, 1.291411025, 1.1827808455, 60.8267871968}, + {0.28, 4, 1.290279857, 1.1827808455, 60.8267871968}, + {0.32, 4, 1.291431043, 1.1827808455, 60.8267871968}, + {0.36, 4, 1.293170653, 1.1827808455, 60.8267871968}, + {0.40, 4, 1.295385827, 1.1827808455, 60.8267871968}, + {0.44, 4, 1.297991738, 1.1827808455, 60.8267871968}, + {0.48, 4, 1.300924032, 1.1827808455, 60.8267871968}, + {0.52, 4, 1.304132086, 1.1827808455, 60.8267871968}, + {0.56, 4, 1.309039395, 1.1827808455, 60.8267871968}, + {0.60, 4, 1.314605487, 1.1827808455, 60.8267871968}, + {0.64, 4, 1.330699437, 1.1827808455, 60.8267871968}, + {0.68, 4, 1.346695167, 1.1827808455, 60.8267871968}, + {0.72, 4, 1.362693183, 1.1827808455, 60.8267871968}, + {0.76, 4, 1.378693074, 1.1827808455, 60.8267871968}, + {0.80, 4, 1.394695194, 1.1827808455, 60.8267871968}, + {0.84, 4, 1.41069913, 1.1827808455, 60.8267871968}, + {0.88, 4, 1.426705046, 1.1827808455, 60.8267871968}, + {0.92, 4, 1.473732816, 1.218959468, 60.91848997}, + {0.96, 4, 1.582395672, 1.291316713, 61.10189551}, + {1.00, 4, 1.681494886, 1.363673958, 61.28530105}, +}; +// Number of entries +#define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0])) // Moonburner motor thrust curve (Sustainer) const std::map moonburner_data = { @@ -41,9 +80,7 @@ const std::map moonburner_data = { {9.993, 207.118}, {10.514, 137.303}, {11.496, 34.908}, - {11.994, 0.0} -}; - + {11.994, 0.0}}; // O5500X motor thrust curve (Booster) std::map O5500X_data = { @@ -86,9 +123,7 @@ std::map O5500X_data = { {3.853, 489.796}, {3.897, 285.714}, {3.981, 20.408}, - {3.997, 0.0} -}; - + {3.997, 0.0}}; /** * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms @@ -109,12 +144,13 @@ std::map O5500X_data = { * */ - -void EKF::initialize(RocketSystems* args) { +void EKF::initialize(RocketSystems *args) +{ Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); float sum = 0; - - for (int i = 0; i < 30; i++) { + + for (int i = 0; i < 30; i++) + { Barometer barometer = args->rocket_data.barometer.getRecent(); LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); Acceleration accelerations = { @@ -123,19 +159,16 @@ void EKF::initialize(RocketSystems* args) { .az = initial_accelerometer.az}; sum += barometer.altitude; - init_accel(0, 0) += accelerations.az; init_accel(1, 0) += accelerations.ay; init_accel(2, 0) += -accelerations.ax; THREAD_SLEEP(100); } - init_accel(0, 0) /= 30; init_accel(1, 0) /= 30; init_accel(2, 0) /= 30; - euler_t euler = orientation.getEuler(); euler.yaw = -euler.yaw; @@ -144,9 +177,7 @@ void EKF::initialize(RocketSystems* args) { x_k(3, 0) = 0; x_k(6, 0) = 0; - - F_mat.setZero(); // Initialize with zeros - + F_mat.setZero(); // Initialize with zeros Q(0, 0) = pow(s_dt, 5) / 20; Q(0, 1) = pow(s_dt, 4) / 8; @@ -158,7 +189,6 @@ void EKF::initialize(RocketSystems* args) { Q(2, 0) = Q(0, 2); Q(2, 1) = Q(1, 2); - Q(3, 3) = pow(s_dt, 5) / 20; Q(3, 4) = pow(s_dt, 4) / 8; Q(3, 5) = pow(s_dt, 3) / 6; @@ -169,7 +199,6 @@ void EKF::initialize(RocketSystems* args) { Q(5, 3) = Q(3, 5); Q(5, 4) = Q(4, 5); - Q(6, 6) = pow(s_dt, 5) / 20; Q(6, 7) = pow(s_dt, 4) / 8; Q(6, 8) = pow(s_dt, 3) / 6; @@ -180,31 +209,24 @@ void EKF::initialize(RocketSystems* args) { Q(8, 6) = Q(6, 8); Q(8, 7) = Q(7, 8); - // set H H(0, 0) = 1; H(1, 2) = 1; H(2, 5) = 1; H(3, 8) = 1; - Q = Q * spectral_density_; - // set R R(0, 0) = 2.0; R(1, 1) = 1.9; R(2, 2) = 10; R(3, 3) = 10; - // set B (don't care about what's in B since we have no control input) B(2, 0) = -1; - - } - /** * @brief Estimates current state of the rocket without current sensor data * @@ -213,70 +235,77 @@ void EKF::initialize(RocketSystems* args) { * it extrapolates the state at time n+1 based on the state at time n. */ - -void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { +void EKF::priori(float dt, Orientation &orientation, FSMState fsm) +{ Eigen::Matrix xdot = Eigen::Matrix::Zero(); Velocity omega = orientation.getVelocity(); euler_t angles = orientation.getEuler(); // Eigen::Matrix gravity = Eigen::Matrix::Zero(); - gravity(0, 0) = -9.81; - float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; h = height_full; + gravity(0, 0) = -9.81; + float m = mass_sustainer; + float h = height_sustainer; + if (fsm < FSMState::STATE_BURNOUT) + { + m = mass_full; + h = height_full; } - float w_x = omega.vx; float w_y = omega.vy; float w_z = omega.vz; - - float J_x = 0.5 * m * r * r; - float J_y = (1/3) * m * h * h + 0.25 * m * r * r; + float w_x = omega.vx; + float w_y = omega.vy; + float w_z = omega.vz; + + float J_x = 0.5 * m * r * r; + float J_y = (1 / 3) * m * h * h + 0.25 * m * r * r; float J_z = J_y; - - float vel_mag_squared = x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0); + float vel_mag_squared = x_k(1, 0) * x_k(1, 0) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0); - float Fax = -0.5*rho*(vel_mag_squared)*float(Ca)*(pi*r*r); + float Fax = -0.5 * rho * (vel_mag_squared) * float(Ca) * (pi * r * r); float Fay = 0; float Faz = 0; + Eigen::Matrix Fg_body; + + EKF::GlobalToBody(angles,Fg_body); - Eigen::Matrix *Fg_body = (GlobalToBody(angles, gravity)); - float Fgx = (*Fg_body)(0,0); float Fgy = (*Fg_body)(1,0); float Fgz = (*Fg_body)(2,0); + float Fgx = Fg_body(0, 0); + float Fgy = Fg_body(1, 0); + float Fgz = Fg_body(2, 0); - delete Fg_body; - Eigen::Matrix * T = EKF::getThrust(stage_timestamp, angles, fsm); - float Ftx = (*T)(0, 0); float Fty = (*T)(1, 0); float Ftz = (*T)(2, 0); + Eigen::Matrix T; - delete T; + EKF::getThrust(stage_timestamp, angles, fsm, T); - - xdot << x_k(1,0), - (Fax + Ftx + Fgx) / m - (w_y * x_k(7,0) - w_z * x_k(4,0)), - 1.0, + float Ftx = T(0, 0); + float Fty = T(1, 0); + float Ftz = T(2, 0); - x_k(4,0), - (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7,0)), - 1.0, + xdot << x_k(1, 0), + (Fax + Ftx + Fgx) / m - (w_y * x_k(7, 0) - w_z * x_k(4, 0)), + 1.0, - x_k(7,0), - (Faz + Ftz + Fgz) / m - (w_x * x_k(4,0) - w_y * x_k(1,0)), - 1.0; + x_k(4, 0), + (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7, 0)), + 1.0, + x_k(7, 0), + (Faz + Ftz + Fgz) / m - (w_x * x_k(4, 0) - w_y * x_k(1, 0)), + 1.0; x_priori = (xdot * dt) + x_k; setF(dt, fsm, w_x, w_y, w_z); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } - /** * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim */ -float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) { +float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) +{ return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); } - /** * @brief Returns the approximate thrust force from the motor given the thurst curve * @@ -291,14 +320,19 @@ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. * The thrust force is then rotated into the body frame using the BodyToGlobal function. */ -Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state) { +void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state,Eigen::Matrix &to_modify) +{ float interpolatedValue = 0; - if (FSM_state >= STATE_FIRST_BOOST){ - if (FSM_state < FSMState::STATE_BURNOUT ) { + if (FSM_state >= STATE_FIRST_BOOST) + { + if (FSM_state < FSMState::STATE_BURNOUT) + { // first stage - if(timestamp >= 0.009){ + if (timestamp >= 0.009) + { auto it = O5500X_data.lower_bound(timestamp); - if (it != O5500X_data.end()) { + if (it != O5500X_data.end()) + { float x0 = it->first; float y0 = it->second; ++it; @@ -307,12 +341,15 @@ Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMS interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); } } - } - else { - if (timestamp >= 0.083){ + } + else + { + if (timestamp >= 0.083) + { // second stage auto it = moonburner_data.lower_bound(timestamp); - if (it != moonburner_data.end()) { + if (it != moonburner_data.end()) + { float x0 = it->first; float y0 = it->second; ++it; @@ -323,11 +360,11 @@ Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMS } } } - Eigen::Matrix interpolatedVector; - interpolatedVector(0,0) = interpolatedValue; - return BodyToGlobal(angles, interpolatedVector); -} + Eigen::Matrix interpolatedVector = Eigen::Matrix::Zero(); + (interpolatedVector)(0, 0) = interpolatedValue; + EKF::BodyToGlobal(angles, interpolatedVector, to_modify); +} /** * @brief Update Kalman Gain and state estimate with current sensor data @@ -337,8 +374,10 @@ Eigen::Matrix *EKF::getThrust(float timestamp, euler_t angles, FSMS * the new sensor data is. After updating the gain, the state estimate is updated. * */ -void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { - if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { +void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) +{ + if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) + { float sum = 0; float data[10]; alt_buffer.readSlice(data, 0, 10); @@ -354,44 +393,36 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori H(1, 2) = 0; } - Eigen::Matrix S_k = Eigen::Matrix::Zero(); S_k = (((H * P_priori * H.transpose()) + R)).inverse(); Eigen::Matrix identity = Eigen::Matrix::Identity(); K = (P_priori * H.transpose()) * S_k; - // Sensor Measurements - Eigen::Matrix accel = Eigen::Matrix::Zero(); - - accel(0, 0) = acceleration.az - 0.045; - accel(1, 0) = acceleration.ay - 0.065; - accel(2, 0) = -acceleration.ax - 0.06; + Eigen::Matrix accel = Eigen::Matrix(Eigen::Matrix::Zero()); + (accel)(0, 0) = acceleration.az - 0.045; + (accel)(1, 0) = acceleration.ay - 0.065; + (accel)(2, 0) = -acceleration.ax - 0.06; euler_t angles = orientation.getEuler(); angles.yaw = -angles.yaw; + Eigen::Matrix acc; + EKF::BodyToGlobal(angles, accel,acc); - Eigen::Matrix * acc = (BodyToGlobal(angles, accel)); - + y_k(1, 0) = ((acc)(0)) * 9.81 - 9.81; + y_k(2, 0) = ((acc)(1)) * 9.81; + y_k(3, 0) = ((acc)(2)) * 9.81; - y_k(1, 0) = ((*acc)(0)) * 9.81 - 9.81; - y_k(2, 0) = ((*acc)(1)) * 9.81; - y_k(3, 0) = ((*acc)(2)) * 9.81; - - delete acc; y_k(0, 0) = barometer.altitude; alt_buffer.push(barometer.altitude); - - - // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); P_k = (identity - K * H) * P_priori; - + kalman_state.state_est_pos_x = x_k(0, 0); kalman_state.state_est_vel_x = x_k(1, 0); kalman_state.state_est_accel_x = x_k(2, 0); @@ -402,13 +433,11 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori kalman_state.state_est_vel_z = x_k(7, 0); kalman_state.state_est_accel_z = x_k(8, 0); - - state.position = (Position){kalman_state.state_est_pos_x,kalman_state.state_est_pos_y,kalman_state.state_est_pos_z}; - state.velocity = (Velocity){kalman_state.state_est_vel_x,kalman_state.state_est_vel_y,kalman_state.state_est_vel_z}; - state.acceleration = (Acceleration){kalman_state.state_est_accel_x,kalman_state.state_est_accel_y,kalman_state.state_est_accel_z}; + state.position = (Position){kalman_state.state_est_pos_x, kalman_state.state_est_pos_y, kalman_state.state_est_pos_z}; + state.velocity = (Velocity){kalman_state.state_est_vel_x, kalman_state.state_est_vel_y, kalman_state.state_est_vel_z}; + state.acceleration = (Acceleration){kalman_state.state_est_accel_x, kalman_state.state_est_accel_y, kalman_state.state_est_accel_z}; } - /** * @brief Run Kalman filter calculations as long as FSM has passed IDLE * @@ -419,9 +448,12 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori * @param &orientation Current orientation * @param current_state Current FSM_state */ -void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) { - if (FSM_state >= FSMState::STATE_IDLE) { - if (FSM_state != last_fsm) { +void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) +{ + if (FSM_state >= FSMState::STATE_IDLE) + { + if (FSM_state != last_fsm) + { stage_timestamp = 0; } stage_timestamp += dt; @@ -433,53 +465,23 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati } } - -/** - * @brief Initializes the aerodynamic data from a CSV file - * - * The function reads a CSV file containing aerodynamic data and stores it in a map. - * The key is the Mach number, and the value is a tuple containing aerodynamic coefficients. - */ -void EKF::initializeAerodynamicData() { - std::ifstream file("LookUp.csv"); - std::string line; - - // Skip the header line - std::getline(file, line); - - while (std::getline(file, line)) { - std::istringstream iss(line); - std::string token; - std::vector row; - - while (std::getline(iss, token, ',')) { - row.push_back(std::stod(token)); - } - - if (row.size() >= 5) { - double mach = std::round(row[0] / 0.04) * 0.04; - aerodynamicData[mach] = std::make_tuple(row[2], row[3], row[4]); - } - } -} - - /** * @brief Getter for state X * * @return the current state, see sensor_data.h for KalmanData */ -KalmanData EKF::getState() { +KalmanData EKF::getState() +{ return state; } - /** * @brief Sets state vector x * * @param state Wanted state vector */ -void EKF::setState(KalmanState state) { +void EKF::setState(KalmanState state) +{ this->state.position.px = state.state_est_pos_x; this->state.position.py = state.state_est_pos_y; this->state.position.pz = state.state_est_pos_z; @@ -491,7 +493,6 @@ void EKF::setState(KalmanState state) { this->state.velocity.vz = state.state_est_vel_z; } - /** * @brief Sets the Q matrix given time step and spectral density. * @@ -501,7 +502,8 @@ void EKF::setState(KalmanState state) { * The Q matrix is the covariance matrix for the process noise and is * updated based on the time taken per cycle of the Kalman Filter Thread. */ -void EKF::setQ(float dt, float sd) { +void EKF::setQ(float dt, float sd) +{ Q(0, 0) = pow(dt, 5) / 20; Q(0, 1) = pow(dt, 4) / 8; Q(0, 2) = pow(dt, 3) / 6; @@ -521,7 +523,6 @@ void EKF::setQ(float dt, float sd) { Q(5, 3) = Q(3, 5); Q(5, 4) = Q(4, 5); - Q(6, 6) = pow(dt, 5) / 20; Q(6, 7) = pow(dt, 4) / 8; Q(6, 8) = pow(dt, 3) / 6; @@ -532,11 +533,9 @@ void EKF::setQ(float dt, float sd) { Q(8, 6) = Q(6, 8); Q(8, 7) = Q(7, 8); - Q *= sd; } - /** * @brief Converts a vector in the body frame to the global frame * @@ -544,22 +543,23 @@ void EKF::setQ(float dt, float sd) { * @param body_vect Vector for rotation in the body frame * @return Eigen::Matrix Rotated vector in the global frame */ -Eigen::Matrix * EKF::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { +void EKF::BodyToGlobal(euler_t angles, Eigen::Matrix &body_vect,Eigen::Matrix &to_modify ) +{ Eigen::Matrix3f roll, pitch, yaw; roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; - - Eigen::Matrix * global = new Eigen::Matrix(yaw * pitch * roll * body_vect); - return global; + + to_modify = yaw * pitch * roll * (body_vect); + } + /** * THIS IS A PLACEHOLDER FUNCTION SO WE CAN ABSTRACT FROM `kalman_filter.h` */ -void EKF::priori(){}; - +void EKF::priori() {}; /** * @brief Converts a vector in the global frame to the body frame @@ -570,19 +570,19 @@ void EKF::priori(){}; * @return Eigen::Matrix Rotated vector in the body frame * */ -Eigen::Matrix * EKF::GlobalToBody(euler_t angles, const Eigen::Matrix world_vector) { - +void EKF::GlobalToBody(euler_t angles, Eigen::Matrix &to_modify) +{ + Eigen::Matrix3f roll; roll << 1, 0, 0, 0, cos(angles.roll), -sin(angles.roll), 0, sin(angles.roll), cos(angles.roll); Eigen::Matrix3f pitch; - pitch << cos(angles.pitch), 0, sin(angles.pitch), 0, 1, 0,-sin(angles.pitch), 0, cos(angles.pitch); + pitch << cos(angles.pitch), 0, sin(angles.pitch), 0, 1, 0, -sin(angles.pitch), 0, cos(angles.pitch); Eigen::Matrix3f yaw; yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1; Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; - (rotation_matrix).transpose(); - Eigen::Matrix * to_return = new Eigen::Matrix(rotation_matrix*world_vector); - return to_return; - + // rotation_matrix(rotation_matrix).transpose(); + to_modify = rotation_matrix.transpose() * gravity; + // return to_return; } /** @@ -594,47 +594,44 @@ Eigen::Matrix * EKF::GlobalToBody(euler_t angles, const Eigen::Matr * by how the states change over time and also depends on the * current state of the rocket. */ -void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) { +void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz) +{ Eigen::Matrix w = Eigen::Matrix::Zero(); w(0, 0) = wx; w(1, 0) = wy; w(2, 0) = wz; - F_mat(0,1) = 1; - F_mat(3,4) = 1; - F_mat(6,7) = 1; + F_mat(0, 1) = 1; + F_mat(3, 4) = 1; + F_mat(6, 7) = 1; + float velocity_magnitude = pow(x_k(1, 0) * x_k(1, 0) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0), 0.5); + float mach = velocity_magnitude / a; + int index = std::round(mach / 0.04); + index = std::clamp(index, 0, (int)AERO_DATA_SIZE - 1); + + Ca = aero_data[index].CA_power_on; + Cn = aero_data[index].CN; + Cp = aero_data[index].CP; float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; - h= height_full; + if (fsm < FSMState::STATE_BURNOUT) + { + m = mass_full; + h = height_full; } + F_mat(1, 1) = -pi * Ca * r * r * rho * x_k(1, 0) / m; + F_mat(1, 4) = -pi * Ca * r * r * rho * x_k(4, 0) / m + w(2, 0); + F_mat(1, 7) = -pi * Ca * r * r * rho * x_k(7, 0) / m - w(1, 0); - F_mat(1,1) = -pi * Ca * r * r * rho * x_k(1,0) / m; - F_mat(1,4) = -pi * Ca * r * r * rho * x_k(4,0) / m + w(2,0); - F_mat(1,7) = -pi * Ca * r * r * rho * x_k(7,0) / m - w(1,0); - - - F_mat(4,1) = pi * Cn * r * r * rho * x_k(1,0) / m - w(2,0); - F_mat(4,4) = pi * Cn * r * r * rho * x_k(2,0) / m + w(0,0); - F_mat(4,7) = pi * Cn * r * r * rho * x_k(3,0) / m; - + F_mat(4, 1) = pi * Cn * r * r * rho * x_k(1, 0) / m - w(2, 0); + F_mat(4, 4) = pi * Cn * r * r * rho * x_k(2, 0) / m + w(0, 0); + F_mat(4, 7) = pi * Cn * r * r * rho * x_k(3, 0) / m; - F_mat(7,1) = pi * Cn * r * r * rho * x_k(1,0) / m + w(1,0); - F_mat(7,4) = pi * Cn * r * r * rho * x_k(2,0) / m - w(0,0); - F_mat(7,7) = pi * Cn * r * r * rho * x_k(3,0) / m; - - - float velocity_magnitude = pow(x_k(1,0)*x_k(1,0) + x_k(4,0)*x_k(4,0) + x_k(7,0)*x_k(7,0), 0.5); - float mach = velocity_magnitude / 343.0; - mach = std::round(mach / 0.04) * 0.04; - std::tie(Ca, Cn, Cp) = aerodynamicData[mach]; - + F_mat(7, 1) = pi * Cn * r * r * rho * x_k(1, 0) / m + w(1, 0); + F_mat(7, 4) = pi * Cn * r * r * rho * x_k(2, 0) / m - w(0, 0); + F_mat(7, 7) = pi * Cn * r * r * rho * x_k(3, 0) / m; } EKF ekf; - - - diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index f874d6e2..b1b43a02 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -3,7 +3,6 @@ #include "kalman_filter.h" #include "sensor_data.h" #include "Buffer.h" -#include #define NUM_STATES 9 #define NUM_SENSOR_INPUTS 4 @@ -20,21 +19,19 @@ class EKF : public KalmanFilter void setQ(float dt, float sd); void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z); - Eigen::Matrix *BodyToGlobal(euler_t angles, Eigen::Matrix x_k); - Eigen::Matrix *getThrust(float timestamp, euler_t angles, FSMState FSM_state); - Eigen::Matrix *GlobalToBody(euler_t angles, const Eigen::Matrix world_vector); + void getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen::Matrix &to_modify); + void BodyToGlobal(euler_t angles, Eigen::Matrix &x_k, Eigen::Matrix &to_modify); + void GlobalToBody(euler_t angles, Eigen::Matrix &to_modify); KalmanData getState() override; void setState(KalmanState state) override; - void initializeAerodynamicData(); float linearInterpolation(float x0, float y0, float x1, float y1, float x); void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state); bool should_reinit = false; private: - std::map> aerodynamicData; float s_dt = 0.05f; float spectral_density_ = 13.0f; float kalman_apo = 0; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 4ca59d68..371ea227 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -1,7 +1,14 @@ #include "systems.h" #include "hal.h" + +#ifdef IS_SUSTAINER #include "gnc/ekf.h" +#endif + +#ifdef IS_BOOSTER +#include "gnc/yessir.h" +#endif #include @@ -180,8 +187,10 @@ DECLARE_THREAD(buzzer, RocketSystems* arg) { } } +#ifdef IS_SUSTAINER DECLARE_THREAD(kalman, RocketSystems* arg) { ekf.initialize(arg); + // Serial.println("Initialized ekf :("); TickType_t last = xTaskGetTickCount(); while (true) { @@ -212,6 +221,43 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { THREAD_SLEEP(50); } } +#endif + +#ifdef IS_BOOSTER +DECLARE_THREAD(kalman, RocketSystems* arg) { + yessir.initialize(arg); + Serial.println("Initialized YESSIR"); + TickType_t last = xTaskGetTickCount(); + + while (true) { + if(arg->rocket_data.command_flags.should_reset_kf){ + yessir.initialize(arg); + TickType_t last = xTaskGetTickCount(); + arg->rocket_data.command_flags.should_reset_kf = false; + } + // add the tick update function + Barometer current_barom_buf = arg->rocket_data.barometer.getRecent(); + Orientation current_orientation = arg->rocket_data.orientation.getRecent(); + HighGData current_accelerometer = arg->rocket_data.high_g.getRecent(); + FSMState FSM_state = arg->rocket_data.fsm_state.getRecent(); + Acceleration current_accelerations = { + .ax = current_accelerometer.ax, + .ay = current_accelerometer.ay, + .az = current_accelerometer.az + }; + float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; + float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; + ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); + KalmanData current_state = ekf.getState(); + + arg->rocket_data.kalman.update(current_state); + + last = xTaskGetTickCount(); + + THREAD_SLEEP(50); + } +} +#endif void handle_tlm_command(TelemetryCommand& command, RocketSystems* arg, FSMState current_state) { // maybe we should move this somewhere else but it can stay here for now From 2b652581c01eca4faeb75a04adb04cd0d80e9e9f Mon Sep 17 00:00:00 2001 From: Keshav B Date: Mon, 17 Feb 2025 18:12:29 -0600 Subject: [PATCH 19/22] Deleted LookUp.csv Co-authored-by: SZhOU-c Co-authored-by: sh1shir Co-authored-by: Rithvik Bhogavilli --- MIDAS/src/gnc/LookUp.csv | 26 -------------------------- 1 file changed, 26 deletions(-) delete mode 100644 MIDAS/src/gnc/LookUp.csv diff --git a/MIDAS/src/gnc/LookUp.csv b/MIDAS/src/gnc/LookUp.csv deleted file mode 100644 index cfd5768a..00000000 --- a/MIDAS/src/gnc/LookUp.csv +++ /dev/null @@ -1,26 +0,0 @@ -Mach,Alpha,CA Power-On,CN,CP -0.04,4,1.332142905,1.1827808455016526632575949416,60.826787196815171703275113218 -0.08,4,1.326587387,1.1827808455016526632575949416,60.826787196815171703275113218 -0.12,4,1.31558627,1.1827808455016526632575949416,60.826787196815171703275113218 -0.16,4,1.306063953,1.1827808455016526632575949416,60.826787196815171703275113218 -0.20,4,1.298117084,1.1827808455016526632575949416,60.826787196815171703275113218 -0.24,4,1.291411025,1.1827808455016526632575949416,60.826787196815171703275113218 -0.28,4,1.290279857,1.1827808455016526632575949416,60.826787196815171703275113218 -0.32,4,1.291431043,1.1827808455016526632575949416,60.826787196815171703275113218 -0.36,4,1.293170653,1.1827808455016526632575949416,60.826787196815171703275113218 -0.40,4,1.295385827,1.1827808455016526632575949416,60.826787196815171703275113218 -0.44,4,1.297991738,1.1827808455016526632575949416,60.826787196815171703275113218 -0.48,4,1.300924032,1.1827808455016526632575949416,60.826787196815171703275113218 -0.52,4,1.304132086,1.1827808455016526632575949416,60.826787196815171703275113218 -0.56,4,1.309039395,1.1827808455016526632575949416,60.826787196815171703275113218 -0.60,4,1.314605487,1.1827808455016526632575949416,60.826787196815171703275113218 -0.64,4,1.330699437,1.1827808455016526632575949416,60.826787196815171703275113218 -0.68,4,1.346695167,1.1827808455016526632575949416,60.826787196815171703275113218 -0.72,4,1.362693183,1.1827808455016526632575949416,60.826787196815171703275113218 -0.76,4,1.378693074,1.1827808455016526632575949416,60.826787196815171703275113218 -0.80,4,1.394695194,1.1827808455016526632575949416,60.826787196815171703275113218 -0.84,4,1.41069913,1.1827808455016526632575949416,60.826787196815171703275113218 -0.88,4,1.426705046,1.1827808455016526632575949416,60.826787196815171703275113218 -0.92,4,1.473732816,1.218959468,60.91848997 -0.96,4,1.582395672,1.291316713,61.10189551 -1.00,4,1.681494886,1.363673958,61.28530105 From 0f14a728e862cc2b58275d62eac3e8e613353357 Mon Sep 17 00:00:00 2001 From: Divij Date: Mon, 17 Feb 2025 19:46:06 -0600 Subject: [PATCH 20/22] added orientation velocity Fixed orientation velocity, was not set before, and sets last_fsm. --- MIDAS/src/gnc/ekf.cpp | 6 ++---- MIDAS/src/hardware/Orientation.cpp | 11 +++++++++++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index e42f4ca4..2b2b986e 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -292,7 +292,6 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) x_k(7, 0), (Faz + Ftz + Fgz) / m - (w_x * x_k(4, 0) - w_y * x_k(1, 0)), 1.0; - x_priori = (xdot * dt) + x_k; setF(dt, fsm, w_x, w_y, w_z); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; @@ -452,16 +451,16 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati { if (FSM_state >= FSMState::STATE_IDLE) { - if (FSM_state != last_fsm) + if (FSM_state != last_fsm) { stage_timestamp = 0; + last_fsm = FSM_state } stage_timestamp += dt; setF(dt, FSM_state, orientation.roll, orientation.pitch, orientation.yaw); setQ(dt, sd); priori(dt, orientation, FSM_state); update(barometer, acceleration, orientation, FSM_state); - FSMState last_fsm = FSM_state; } } @@ -580,7 +579,6 @@ void EKF::GlobalToBody(euler_t angles, Eigen::Matrix &to_modify) Eigen::Matrix3f yaw; yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1; Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; - // rotation_matrix(rotation_matrix).transpose(); to_modify = rotation_matrix.transpose() * gravity; // return to_return; } diff --git a/MIDAS/src/hardware/Orientation.cpp b/MIDAS/src/hardware/Orientation.cpp index 197d416b..d59749b4 100644 --- a/MIDAS/src/hardware/Orientation.cpp +++ b/MIDAS/src/hardware/Orientation.cpp @@ -192,10 +192,21 @@ Orientation OrientationSensor::read() sensor_reading.pitch = euler.x; sensor_reading.roll = euler.z; + + + sensor_reading.linear_acceleration.ax = -event.un.accelerometer.y; sensor_reading.linear_acceleration.ay = event.un.accelerometer.x; sensor_reading.linear_acceleration.az = event.un.accelerometer.z; + Velocity velocity; + velocity.vx = sensor_reading.linear_acceleration.ax * deltaTime + velocity.vx; + velocity.vy = sensor_reading.linear_acceleration.ay * deltaTime + velocity.vy; + velocity.vz = sensor_reading.linear_acceleration.az * deltaTime + velocity.vz; + + + sensor_reading.orientation_velocity = velocity; + sensor_reading.gx = -event.un.gyroscope.y; sensor_reading.gy = event.un.gyroscope.x; sensor_reading.gz = event.un.gyroscope.z; From db3a23ef9ff224409920e40ee3539c177f78ee82 Mon Sep 17 00:00:00 2001 From: Keshav B Date: Mon, 17 Feb 2025 19:53:58 -0600 Subject: [PATCH 21/22] dont forget semicolons --- MIDAS/src/gnc/ekf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 2b2b986e..8ca99604 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -454,7 +454,7 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati if (FSM_state != last_fsm) { stage_timestamp = 0; - last_fsm = FSM_state + last_fsm = FSM_state; } stage_timestamp += dt; setF(dt, FSM_state, orientation.roll, orientation.pitch, orientation.yaw); From 559ee4d671a4a2e79df2bcb765e08b1fa52f7829 Mon Sep 17 00:00:00 2001 From: Michael K Date: Mon, 17 Feb 2025 19:58:58 -0600 Subject: [PATCH 22/22] fix booster compilation issue --- MIDAS/src/systems.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 371ea227..34965d5b 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -247,8 +247,8 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { }; float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; - ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); - KalmanData current_state = ekf.getState(); + yessir.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); + KalmanData current_state = yessir.getState(); arg->rocket_data.kalman.update(current_state);