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

tools/para/conv2_w

@@ -0,0 +1,544 @@
+6
+10
+3
+3
+-0.18567735
+0.31230295
+0.10601954
+-0.14635935
+0.17529164
+0.23833705
+-0.07737787
+-0.26470512
+-0.016763074
+0.0732159
+0.15613954
+0.24339953
+0.21809842
+0.10773516
+0.40015697
+0.08531675
+0.25792578
+0.29302046
+-0.14973466
+-0.13794622
+-0.3174705
+0.06396363
+-0.1582382
+-0.2843077
+-0.25538033
+-0.23979947
+-0.2003432
+-0.067921035
+0.008744139
+-0.12954548
+0.07350555
+0.016850626
+-0.13139062
+0.07412223
+-0.07210239
+-0.042953018
+-0.3169445
+0.009288535
+0.30465534
+-0.4208047
+0.07115591
+0.26925838
+-0.38791
+0.06488703
+0.400365
+-0.5090438
+-0.05041841
+0.28737134
+-0.32649037
+0.2732965
+0.3234882
+-0.14768949
+0.19571733
+0.46035185
+-0.1308439
+-0.28924602
+-0.4150856
+-0.1394865
+-0.14277102
+-0.028665107
+0.063959725
+-0.08086398
+-0.42271927
+-1.0998808
+-1.7531674
+-0.6868419
+-0.4679067
+-1.1683141
+-0.3862623
+0.1865768
+-0.7758855
+0.18598925
+-0.2777104
+-0.5689085
+-0.53544736
+0.06992068
+0.09683173
+-0.05572889
+0.497631
+0.5235087
+0.412554
+0.35491413
+0.20174716
+0.14414282
+0.21999303
+0.14763339
+0.15525024
+0.13345838
+-0.07630705
+-0.007321237
+-0.17791471
+0.067984045
+0.07404319
+-0.2620913
+-0.020879569
+-0.09856246
+-0.27126157
+-0.19668914
+-0.45068544
+0.06535956
+0.19665721
+0.4605025
+0.0834005
+0.26365396
+0.42590958
+0.041509036
+0.28209603
+0.23930955
+-0.16162594
+-0.22635049
+-0.26243812
+-0.12631208
+-0.22097321
+-0.20312756
+-0.1772115
+-0.33603805
+-0.30561772
+0.008207564
+0.07136626
+-0.216243
+0.16515563
+0.04111277
+-0.1260301
+0.14099488
+0.10370603
+-0.010030857
+-0.27448004
+-0.011123834
+0.22102898
+-0.25228068
+0.039984487
+0.24022454
+-0.12004822
+0.10729556
+0.22277792
+-0.2681527
+0.1929927
+0.35833898
+-0.1897413
+0.2386225
+0.59820104
+-0.27273512
+0.1098645
+0.36686987
+-0.43986773
+-0.18380699
+-0.43966547
+-0.009675484
+-0.25631252
+-0.29056635
+0.0712151
+-0.18025662
+-0.48891056
+-0.47694656
+-0.88816905
+-0.06483537
+0.013368553
+-0.70404077
+0.28231522
+0.33836752
+-0.22945245
+0.25680065
+-0.0012361507
+-0.52792823
+-0.2579657
+0.3255992
+-0.0019560119
+-0.046853587
+0.24827203
+0.34998816
+0.27723187
+0.31715482
+0.34362832
+0.12269884
+0.13557787
+-0.114973396
+0.07401403
+0.069110855
+-0.187023
+-0.036038406
+0.012674593
+0.36098468
+0.42171153
+0.048450485
+0.28105047
+0.4715788
+-0.023357779
+-0.057762705
+-0.073359944
+-0.16736583
+0.12476493
+0.0062710703
+-0.021589287
+-0.13475952
+0.030259239
+-0.036233522
+0.06367138
+-0.07908364
+0.36171123
+0.056995805
+0.26451215
+0.18291938
+0.06195444
+0.23869722
+0.10497623
+0.09330571
+0.29085782
+-0.2596862
+-0.031500176
+-0.013901459
+0.023374349
+0.016828336
+0.11964698
+0.026195215
+0.035629973
+0.12736408
+-0.42047524
+-0.045428358
+0.4764139
+-0.594475
+-0.14393432
+0.3904298
+-0.18805574
+-0.08113497
+0.5267287
+-0.4067696
+-0.32341024
+-0.29679683
+-0.10745416
+0.08389849
+0.14507549
+-0.024750415
+0.4222244
+0.16702761
+-0.008214667
+0.03591805
+0.08528825
+0.12195521
+0.41446808
+0.46344396
+0.37176162
+0.40626523
+0.08781661
+-0.8886079
+-1.0024366
+-0.86886346
+-0.49839306
+-0.8621685
+-0.642396
+-0.13097173
+-0.7362578
+-0.22594142
+-0.07626368
+-0.5901063
+-0.3290703
+0.14976943
+-0.06745293
+-0.0820076
+0.5327264
+0.4384248
+0.6160922
+0.24003918
+0.24436
+0.22210835
+0.21770608
+0.124687046
+0.0470902
+-0.14859878
+-0.321342
+-0.08767322
+0.20693085
+0.42665282
+0.33726025
+-0.020886194
+0.44385502
+0.01808433
+-0.118666425
+-0.15688846
+-0.31219846
+0.20918387
+0.021502182
+0.33103526
+0.24867874
+0.21629289
+0.013217075
+0.08715608
+0.054708257
+-0.15127313
+0.1508026
+0.24620876
+0.44316766
+0.17231683
+0.1236346
+0.26164678
+-0.08726074
+-0.052426733
+0.33535057
+0.36311948
+0.41015378
+0.34961388
+0.09574385
+0.5627171
+0.4841079
+0.36436325
+0.62349635
+0.651723
+-0.4531835
+0.15029465
+0.57511115
+-0.45798486
+0.19631258
+0.3225775
+-0.103153236
+0.19025934
+0.48245525
+-0.09091705
+-0.094119325
+-0.38689196
+0.14930637
+0.09523474
+-0.04373269
+0.3126633
+0.08655161
+-0.082390875
+-0.053303
+0.31841794
+0.3719926
+0.15074554
+0.40985286
+0.33657342
+0.20797513
+0.24987732
+0.39347094
+0.83306986
+0.86048186
+0.5689052
+0.72829825
+0.89263994
+0.8195005
+0.61129767
+0.78122336
+0.61171514
+-0.319987
+-0.25968623
+-0.11498821
+0.23861922
+0.088788316
+0.2950782
+0.62382466
+0.8079981
+0.61313707
+0.077279635
+-0.032932185
+0.0890238
+0.029694919
+0.12197181
+-0.042123534
+-0.29403502
+-0.36851963
+0.09636119
+0.04346845
+0.43912384
+0.3536572
+0.066943154
+0.4024961
+0.47593385
+0.0759621
+0.39561898
+0.22179781
+-0.35304305
+-0.20236854
+-0.008968132
+-0.09802803
+0.054423235
+0.23025425
+-0.015376655
+0.008936614
+0.2355261
+0.0014180358
+0.17833155
+0.19288565
+0.28466016
+-0.02708328
+0.05748906
+0.115661204
+-0.08906293
+0.06937998
+-0.12015559
+-0.11914039
+-0.15230455
+-0.30971813
+0.12253699
+-0.1287006
+-0.11642937
+-0.03524333
+0.106573164
+-0.46892896
+0.048909124
+0.35770398
+-0.48432148
+0.06230857
+0.35268116
+-0.6384595
+0.054048087
+0.33101568
+-0.7891181
+-0.5678995
+-0.32257825
+-0.30838996
+-0.019490078
+0.20418139
+0.2235779
+0.43282986
+0.33755836
+-0.43536973
+-0.18715984
+-0.017235223
+-0.22174615
+0.009737464
+0.18385953
+0.3702021
+0.1722581
+0.31182298
+-1.5388889
+-1.4710528
+-0.9808655
+-1.2328979
+-1.391655
+-0.9775159
+-0.53418314
+-1.0619466
+-0.14114031
+-0.38471037
+-0.91709787
+-0.6730362
+-0.3246664
+-0.50130564
+-0.29241592
+0.2488604
+0.29896885
+0.42214942
+0.23901664
+0.071325876
+0.0730879
+0.3215266
+0.37580463
+0.17601205
+0.082663685
+-0.14512439
+-0.02447424
+0.036292106
+0.190385
+0.24941531
+-0.15101732
+0.16009882
+0.022328991
+-0.13642876
+-0.2557106
+-0.23009326
+-0.2648493
+0.035393387
+0.058671933
+-0.06313441
+0.085322335
+0.037474677
+-0.09684385
+0.021198915
+-0.23987578
+0.49218395
+0.45345736
+0.4600616
+0.27070585
+0.23068464
+0.37693858
+0.32365304
+0.2629329
+0.38881606
+0.25656843
+0.3796918
+0.45741585
+0.08370856
+0.29546905
+0.55899864
+0.16390188
+0.43054807
+0.6232953
+-0.6125586
+0.042835608
+0.42402682
+-0.4055307
+-0.039988685
+0.42992234
+-0.26697057
+0.10284304
+0.4822276
+-0.32137555
+-0.38547745
+-0.33756226
+0.12396629
+0.018000755
+0.0040020915
+0.09871851
+0.19964874
+-0.049439877
+-0.054510776
+0.20413962
+0.49790758
+0.3678443
+0.47925648
+0.5145687
+0.46790117
+0.4268048
+0.23691703
+0.7796671
+0.6537224
+0.6620804
+0.6199029
+0.6739477
+0.5245343
+0.7053961
+0.5178989
+0.678157
+0.015971098
+-0.09368621
+-0.1418303
+0.2623539
+0.27078906
+0.26214978
+0.450224
+0.5519793
+0.589996
+0.0695408
+0.11883035
+0.16186124
+-0.00019518172
+-0.10749374
+-0.07061164
+-0.37467608
+-0.33476043
+-0.19680664