对上交代码进行修改，主要将能量机关去掉，添加了同济的PnP位姿解算，但是同济有个四元数，获取IMU部分没有启用，可能导致精度不够。当前还存在反陀螺功能，修改为逻辑和弹道预测相结合，主要在时间关系上进行调整。

tools/para/fc1_b

@@ -0,0 +1,61 @@
+60
+-0.193765
+-0.23972291
+0.07138973
+0.32630488
+-0.12837179
+0.15837212
+-0.171342
+0.15245913
+0.2655143
+0.5132081
+-0.36182725
+0.5721853
+1.1356937
+0.478751
+-0.19061308
+-0.4349855
+-0.037231974
+0.82266986
+0.021297721
+0.27819484
+-0.3700498
+-0.080103956
+0.118309975
+-0.120765805
+-0.21436372
+0.38247055
+0.9884826
+-0.4042391
+-0.21405952
+-1.1451919
+-0.07339987
+0.63600576
+0.39354753
+-0.14708714
+0.5753427
+-0.06053154
+0.6068468
+-0.24616803
+-0.18896966
+-0.06904445
+-0.137959
+-0.75066173
+-0.20227952
+0.17195949
+-0.13245444
+-0.10060697
+0.40073013
+-0.9563751
+0.031663556
+0.38514042
+-0.061627824
+0.14056061
+0.7515243
+0.7903934
+0.8401122
+0.29314804
+0.2987165
+0.39764544
+0.3022464
+-0.29335007