localization模块中msf定位方式漂移严重，如何正确配置该定位方式。 #15713
Description
我在测试msf定位模块，我的数据在rtk和ndt两种模式下都可以正常定位，但是在msf定位模式下，雷达定位就很不稳定，经常会漂移很大的距离，特别是在车辆转弯的时候，msf使用的地图，我按说明，用apollo10.0和apollo5.0.0都测试过，相对而言apollo5.0.0版本生成的lossy_map稍有改善，不过也是无法稳定的运行。我感觉还是IMU数据有问题，log输出如下，sins_模块输出的有时候明显有问题，如Mean diff：nan。我在尝试将定位中使用到的lossy_map提取出来，看一下是不是还是地图的问题。针对msf模块有什么前置条件要满足吗？希望您能给点建议，谢谢（3Q）
最后有我的log输出文件（抱拳）
modules/localization/msf/local_integ/lidar_msg_transfer.cc:95 save online pts. online_points.size(): 164830
modules/localization/msf/local_integ/localization_lidar_process.cc:302 lidar_status_ == LidarState::NOT_STABLE
get init pva pose of current velodyne time from pva buffer
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:490 *****
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:126 *****
entropy 2.614146490259352
discret_cloud size: 28692
altitude_param a and b: -0.04425467135010509 -0.01985567153739872
possible unexpected huge altitude_param a && b: -0.04425467135010509 -0.01985567153739872
delta_yaw: 0.01354884635657072 , cur limit: 0.02617991666666666
PCD Projeciton Elapsed time: 17158us
Mean diff: -1.115355491638184
Call calculate_sad_avx
SAD computation Elapsed time: 59475us
SAD Sum: 7.916603700306352e-38
SAD Sum: 1.52560944729193e-63
Weight debug: 2.07519922053359, 3.245534207858206, 6.675019353337231, 10.89850244659244, 0.7595823769160321, 0.2404176230839679
SAD Sum: 0.2923961226611544
KL 6.657101167822348 inverse 0.1502155329760622
valid ratio: 0.752267657992565
Range: 0, 9, 5, 15
Std of location: std_x = 0.251713223034023, std_y = 0.0788913606111861
Compute localization Elapsed time: 163us
Others Elapsed time: 7us
The valid ratio is lower than given thresold.
the current imu time and measure time: 1744682615.143919 1744682615.133488
the chi is large: 3.841000 13.327121
the chi: 0 3.841000 13.327121
time of integrated navigation measure update Elapsed time: 20343us
modules/localization/msf/local_integ/lidar_msg_transfer.cc:60 original msg.point_size() :230400
modules/localization/msf/local_integ/lidar_msg_transfer.cc:95 save online pts. online_points.size(): 166019
modules/localization/msf/local_integ/localization_lidar_process.cc:302 lidar_status_ == LidarState::NOT_STABLE
get init pva pose of current velodyne time from pva buffer
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:490 *****
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:126 *****
entropy 3.646419398789216
discret_cloud size: 29852
altitude_param a and b: -0.03058458918046551 -0.008728224947771108
delta_yaw: 0.01365534029901028 , cur limit: 0.02617991666666666
PCD Projeciton Elapsed time: 17242us
Mean diff: -0.1290435045957565
Call calculate_sad_avx
SAD computation Elapsed time: 60886us
SAD Sum: 1.343708281828611e-33
SAD Sum: 1.525041589644578e-53
Weight debug: 1.602919742538306, 1.159620194379407, 4.794922875601043, 4.639921122759409, 0.4178775378278168, 0.5821224621721832
SAD Sum: 0.8891942849256684
KL 6.201369513213887 inverse 0.1612547031537467
valid ratio: 0.7861760229205838
Range: 3, 13, 4, 14
Std of location: std_x = 0.1056910022157304, std_y = 0.06537425211973596
Compute localization Elapsed time: 174us
Others Elapsed time: 8us
The valid ratio is lower than given thresold.
the current imu time and measure time: 1744682615.653218 1744682615.642892
time of integrated navigation measure update Elapsed time: 19513us
the current imu time and measure time: 1744682615.663678 1744682615.663658
the chi is large: 7.815000 715.448931
the chi: 0 3.841000 585.724727
the chi: 1 3.841000 8.816231
the chi: 2 3.841000 121.548054
the chi is large: 5.991000 166.777568
the chi: 0 3.841000 7.641685
the chi: 1 3.841000 159.004317
time of integrated navigation measure update Elapsed time: 10932us
modules/localization/msf/local_integ/lidar_msg_transfer.cc:60 original msg.point_size() :230400
modules/localization/msf/local_integ/lidar_msg_transfer.cc:95 save online pts. online_points.size(): 166582
modules/localization/msf/local_integ/localization_lidar_process.cc:302 lidar_status_ == LidarState::NOT_STABLE
get init pva pose of current velodyne time from pva buffer
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:490 *****
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:126 *****
entropy 2.686646897451678
discret_cloud size: 30310
altitude_param a and b: -0.02042315566590025 0.002706220156576496
delta_yaw: 0.008952049538493156 , cur limit: 0.02617991666666666
PCD Projeciton Elapsed time: 18567us
Mean diff: 0.3326683640480042
Call calculate_sad_avx
SAD computation Elapsed time: 61764us
SAD Sum: 1.658282124828596e-29
SAD Sum: 2.551223626561156e-45
Weight debug: 1.261591255229232, 0.5751631450359236, 2.809429347036584, 2.854531913654833, 0.3519867147315696, 0.6480132852684304
SAD Sum: 0.9311002398063143
KL 5.325157284219383 inverse 0.1877878805501968
valid ratio: 0.8262504172449535
Range: 5, 15, 6, 16
Std of location: std_x = 0.07032004048277717, std_y = 0.06560189492092695
Compute localization Elapsed time: 176us
Others Elapsed time: 8us
the current imu time and measure time: 1744682615.901155 1744682615.799909
the chi is large: 7.815000 309.362921
the chi: 0 3.841000 8.613296
the chi: 1 3.841000 6.565396
the chi: 2 3.841000 294.933574
the chi is large: 3.841000 11.544456
the chi: 0 3.841000 11.544456
time of integrated navigation measure update Elapsed time: 3130us
modules/localization/msf/local_integ/lidar_msg_transfer.cc:60 original msg.point_size() :230400
modules/localization/msf/local_integ/lidar_msg_transfer.cc:95 save online pts. online_points.size(): 164269
modules/localization/msf/local_integ/localization_lidar_process.cc:299 lidar_status_ == LidarState::OK
the wheelspeed data list is empty!
the wheelspeed data buffer is not ok!
get forecastpose from imu and wheelspeed fail and begin get from imu
the latest imu and end time: 1744682616.2272 1744682616.19991
get forecastpva from imu dead recking!
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:490 *****
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:126 *****
entropy 2.42409315409021
discret_cloud size: 30069
altitude_param a and b: -0.0155814937833075 0.00494421497626811
delta_yaw: 0.00303200702182949 , cur limit: 0.0239982569444444
PCD Projeciton Elapsed time: 23966us
Mean diff: 1.1507716178894
Call calculate_sad_avx
SAD computation Elapsed time: 60501us
SAD Sum: 1.10749772368115e-28
SAD Sum: 6.29522406417144e-43
Weight debug: 1.41181979470433, 0.630200762097226, 3.11373071950427, 3.88503528451724, 0.417879132664125, 0.582120867335875
SAD Sum: 0.891134913292815
KL 6.23828552914858 inverse 0.160300453598584
valid ratio: 0.839325162117865
Range: 5, 15, 6, 16
Std of location: std_x = 0.0739010511638535, std_y = 0.0725078362230968
Compute localization Elapsed time: 174us
Others Elapsed time: 8us
the current imu time and measure time: 1744682616.306296 1744682616.199913
the chi is large: 7.815000 338.056802
the chi: 2 3.841000 335.860279
the chi is large: 3.841000 19.857478
the chi: 0 3.841000 19.857478
time of integrated navigation measure update Elapsed time: 3018us
the current imu time and measure time: 1744682616.572368 1744682616.572336
the chi is large: 5.991000 2999.115132
the chi: 0 3.841000 2972.158898
the chi: 1 3.841000 27.285390
time of integrated navigation measure update Elapsed time: 11058us
modules/localization/msf/local_integ/lidar_msg_transfer.cc:60 original msg.point_size() :230400
modules/localization/msf/local_integ/lidar_msg_transfer.cc:95 save online pts. online_points.size(): 165136
modules/localization/msf/local_integ/localization_lidar_process.cc:299 lidar_status_ == LidarState::OK
the wheelspeed data list is empty!
the wheelspeed data buffer is not ok!
get forecastpose from imu and wheelspeed fail and begin get from imu
the latest imu and end time: 1744682616.62104 1744682616.59992
get forecastpva from imu dead recking!
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:490 *****
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:126 *****
entropy 2.59373034274763
discret_cloud size: 30229
altitude_param a and b: -0.00808115022587996 0.0129487104099641
delta_yaw: -0.000371982168871909 , cur limit: 0.0218165972222222
PCD Projeciton Elapsed time: 18406us
Mean diff: 1.62720108032227
Call calculate_sad_avx
SAD computation Elapsed time: 62944us
SAD Sum: 3.08511127122165e-28
SAD Sum: 1.41370349678534e-42
Weight debug: 1.83777783934202, 0.309316471991502, 2.97538541876137, 2.9260818615756, 0.195980307728631, 0.804019692271369
SAD Sum: 0.938159467191399
KL 5.65574818100805 inverse 0.176811266696419
valid ratio: 0.847345838800043
Range: 5, 15, 6, 16
Std of location: std_x = 0.0499724167220651, std_y = 0.0482511890948464
Compute localization Elapsed time: 177us
Others Elapsed time: 7us
the current imu time and measure time: 1744682616.704497 1744682616.599923
the chi is large: 7.815000 363.086523
the chi: 1 3.841000 4.057883
the chi: 2 3.841000 355.104949
the chi is large: 3.841000 17.670723
the chi: 0 3.841000 17.670723
time of integrated navigation measure update Elapsed time: 2840us
modules/localization/msf/local_integ/lidar_msg_transfer.cc:60 original msg.point_size() :230400
modules/localization/msf/local_integ/lidar_msg_transfer.cc:95 save online pts. online_points.size(): 163806
modules/localization/msf/local_integ/localization_lidar_process.cc:299 lidar_status_ == LidarState::OK
the wheelspeed data list is empty!
the wheelspeed data buffer is not ok!
get forecastpose from imu and wheelspeed fail and begin get from imu
the latest imu and end time: 1744682617.00383 1744682616.99991
get forecastpva from imu dead recking!
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:490 *****
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:126 *****
entropy 2.32608963481878
discret_cloud size: 30119
altitude_param a and b: -0.0103846762457324 0.023329591336576
delta_yaw: -0.00269188662059605 , cur limit: 0.0196349375
PCD Projeciton Elapsed time: 17094us
Mean diff: 2.84500694274902
Call calculate_sad_avx
SAD computation Elapsed time: 59751us
SAD Sum: 2.16266879374382e-28
SAD Sum: 1.30815448586059e-43
Weight debug: 22.0868173410167, 0.98022690567698, 6.20510332190991, 3.49116680936812, 0.0218600341229063, 0.978139965877094
SAD Sum: 0.97652189860696
KL 6.1588767758118 inverse 0.162367268643427
valid ratio: 0.852691268804894
Range: 5, 15, 6, 16
Std of location: std_x = 0.0835635654207478, std_y = 0.0807760243136692
Compute localization Elapsed time: 176us
Others Elapsed time: 9us
the current imu time and measure time: 1744682617.092961 1744682616.999912
the chi is large: 7.815000 405.197937
the chi: 1 3.841000 4.375356
the chi: 2 3.841000 398.848713
the chi is large: 3.841000 9.189383
the chi: 0 3.841000 9.189383
time of integrated navigation measure update Elapsed time: 2783us
E0707 09:01:48.299645 1119350 msf_localization_component.cc:180] [localization]the localization processing time enlonged more than 2 times according to system time, the pre system time and current system time: 1751850108.273645 1751850108.29957
modules/localization/msf/local_integ/lidar_msg_transfer.cc:60 original msg.point_size() :230400
modules/localization/msf/local_integ/lidar_msg_transfer.cc:95 save online pts. online_points.size(): 164945
modules/localization/msf/local_integ/localization_lidar_process.cc:299 lidar_status_ == LidarState::OK
the quary timestamp, first pva buffer and back pva time: 1744682616.999912 1744682617.2543 1744682617.431144
The first pva time is large than the quary time
the inspva buffer is not ok!
get the pre time pose from inspva buffer fail and get current time pose from pva buffer
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:490 *****
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:126 *****
entropy 2.806635784576355
discret_cloud size: 29912
altitude_param a and b: -0.001197043701269754 0.0229461312373107
delta_yaw: 0.003324953839182854 , cur limit: 0.02617991666666666
PCD Projeciton Elapsed time: 18024us
Mean diff: 3.841500997543335
Call calculate_sad_avx
the current imu time and measure time: 1744682617.472598 1744682617.472573
the chi is large: 5.991000 9456.434974
the chi: 0 3.841000 9231.133261
the chi: 1 3.841000 226.386488
time of integrated navigation measure update Elapsed time: 10954us
SAD computation Elapsed time: 60440us
SAD Sum: 1.739653769814662e-25
SAD Sum: 8.40017444760565e-41
Weight debug: 3.719266323285485, 17.40145811009654, 9.440974456297804, 5.103623728445309, 0.640932718577784, 0.3590672814222159
SAD Sum: 0.7575098130318844
KL 6.954391926512002 inverse 0.1437940240594915
valid ratio: 0.8770736472673988
Range: 11, 19, 10, 19
Std of location: std_x = 0.1740223256095043, std_y = 0.115717831301598
Compute localization Elapsed time: 145us
Others Elapsed time: 7us
the current imu time and measure time: 1744682617.50929 1744682617.399913
the chi is large: 7.815000 480.443046
the chi: 0 3.841000 16.881117
the chi: 1 3.841000 27.622853
the chi: 2 3.841000 434.312394
the chi is large: 5.991000 9563.778677
the chi: 0 3.841000 9327.814676
the chi: 1 3.841000 237.080827
time of integrated navigation measure update Elapsed time: 3477us
modules/localization/msf/local_integ/lidar_msg_transfer.cc:60 original msg.point_size() :230400
modules/localization/msf/local_integ/lidar_msg_transfer.cc:95 save online pts. online_points.size(): 161384
modules/localization/msf/local_integ/localization_lidar_process.cc:299 lidar_status_ == LidarState::OK
the wheelspeed data list is empty!
the wheelspeed data buffer is not ok!
get forecastpose from imu and wheelspeed fail and begin get from imu
the latest imu and end time: 1744682617.80134 1744682617.79991
get forecastpva from imu dead recking!
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:490 *****
modules/localization/msf/local_pyramid_map/base_map/base_map.cc:126 *****
entropy 3.17100258950031
discret_cloud size: 29572
altitude_param a and b: 0.00482518620556089 0.0216957394818653
delta_yaw: 0.00616108626127243 , cur limit: 0.0239982569444444
PCD Projeciton Elapsed time: 24032us
Mean diff: 5.30570030212402
Call calculate_sad_avx
SAD computation Elapsed time: 58655us
SAD Sum: 7.9405793882498e-26
SAD Sum: 3.04800937171587e-40
Weight debug: 2.72894300995983, 12.4333655435116, 5.50832872490124, 4.97379907472116, 0.787276077220534, 0.212723922779466
SAD Sum: 0.835615341598228
KL 3.49889776487092 inverse 0.285804292437476
valid ratio: 0.877311482670801
Range: 4, 14, 4, 14
Std of location: std_x = 0.137092489718313, std_y = 0.117244943648678
Compute localization Elapsed time: 175us
Others Elapsed time: 8us
the current imu time and measure time: 1744682617.906006 1744682617.799914
the chi is large: 7.815000 488.766785
the chi: 0 3.841000 12.454387
the chi: 1 3.841000 10.411452
the chi: 2 3.841000 464.274006
the chi is large: 3.841000 11.098665
the chi: 0 3.841000 11.098665
time of integrated navigation measure update Elapsed time: 3993us