RTK configuration gone wrong

[ZakariaLaakel]

Hello MRS team,

I have been working on the drone setup for flying with RTK as altitude and lat_estimator. I have started again from scratch, pulled the current version of mrs_workspace (9/04) and built, in the tmux_scripts, a folder with my custom_configs folder where I put several yaml files to modify the wanted parameters. I still have the same problem as before (issue #49 ) where the XYZ position coming from the /uav*/uav_state is going wild, and do not stabilize. I have put a figure obtained from the plotjuggler, where you can see that the uav_state/pose/position is trying converge towards the rtk_local_odom/pose , but with a high settling time. Why is this happening? I also observe the /uav_state/pose/position/z component decrease linearly, why does the /uav_state/pose/position/x and y component do not behave the same way?

I wonder what I could have been doing wrong.

I leave you my custom_configs in zip here.

Best regards

Zakaria

just_flying_rtk.zip

[petrlmat]

Hi, sorry for the late answer. Unfortunately, I cannot debug it from your custom configs only. Could you please also attach rosbags and tmux logs from the flight? There are multiple issues that might have caused this behavior:

• The RTK corrections are not fused at all
• The RTK corrections are fused with too high covariance
• The quality of the onboard GPS is very bad (Do you use any USB3 devices in your setup?)

Anyhow, the rosbag and tmux log would tell me much more, now I am just listing the most common problems.

[ZakariaLaakel]

Hi @petrlmat,

I've attached the bag file and the logs to this message. We are using the intel NUC 10 - NUC10i7FNK and we are connecting the RTK (Reach M2) to 1 of the rear usb ports (which are indeed usb3.1 ports) and the ftdi board for the pixhawk-nuc connection to the front usb port, also 3.1 port. RTKlogsRosbag.zip

Kind regards,

Zakaria

[petrlmat]

I analyzed your bag files and the gps-rtk fusion looks ok. Both are providing different position measurements so the estimate is somewhere between them depending on the covariance matrices as one would expect. The current covariance matrices were tuned with a much worse RTK that we were using previously. Furthermore, we were using the RTK measurements to evaluate the precision of localization algorithms, measure position of objects etc., but not so much for precise state estimation, so we didn't mind the soft fusion.

One thing you can do to move the estimate closer to the RTK measurements is to modify the covariance matrices. I ran the estimation on your bag file and ended up with values, which you can put into your odometry custom config. It is not ideal and I don't know how it will behave in feedback, so watch out.

lateral:
  rtk:
    R: [0.001, 0,
        0, 0.001]
    Q: [10, 0,
        0, 10]

As for the Z component, I will have to look into it the next week. Again, we were always flying with a rangefinder, so the RTK altitude estimation was never used by us and it was added just recently without any testing on real hardware.

[ZakariaLaakel]

Hello @petrlmat,

Thank you very much for your answer. Indeed the drone is now much more stable in the XY plane and we were able to let the drone take off and hover on a fixed position in the XY plane (the height was still a bit shitty due to the barometer).

May I ask you how you tested this? This can be something useful to know. So if I understood correctly, with the default gains the focus was more on the information received by the gps rather than the rtk?

Can we test the influence of these parameters in the simulation or is it impossible? Because I have the feeling when we use rtk in the simulation it is not fused with anything else. Is it possible to have the same behaviour during the real experiment (have just pure RTK)? Or is this too unsafe?

For the RTK altitude: we want to try to tune the KF coefficients on our own as well. We saw that the rtk altitude estimator is fusing 'vel_baro' with the 'height_rtk': Is it the altitude: R: height_rtk: [100.0] vel_baro: [100.0] that we have to change? In our case decreasing the R value for the height_rtk should be enough to give us accurate result in the Z direction?

Thank's again for your tips.

Zakaria

[petrlmat]

Hi, glad to hear that the XY estimation was improved. The tuning is actually quite easy to do on rosbag. These are the necessary steps:

• Run the rosbag with the --clock argument, which causes it to use the ROS time from the rosbag so that measurements are fused with correct timestamp.
• Set rosparam set use_sim_time true so that the odometry node indeed uses the time published by the rosbag.
• Remap the topic of uav_state in odometry.launch to something else, so that it is not published to the same topic as is already recorded in the rosbag.
• Change the parameters you want to tune in config files.
• Run the odometry by roslaunch mrs_uav_odometry odometry.launch
• Plot the relevant data in plotjuggler.

Yes, by default the pixhawk GPS position measurements have much higher confidence.

It is possible to test it in simulation, but beware that the data in simulation is different from real data and many errors in sensor measurements such as discrete jumps or dropouts are not modeled, so if you tune the estimator in simulation, the same values might not work with real hardware.

It might be possible to use pure RTK if you are sure that the data never stop coming. Also it is important to check whether there are discrete jumps in the RTK position when RTK fix is lost or whether the transition is smooth. If there is any jump in the position, the UAV will likely quickly land in a controlled way (eland) and if the RTK data stop arriving to the odometry node the UAV will perform sort of a controlled fall (failsafe), which might be destructive from higher altitudes.

Yes, by decreasing the R value you are essentially telling the estimator to trust the measurement more. Let me know, if it helps.

[ZakariaLaakel]

Hey, we were able to fix the Z component as well by reducing the R matrix of the height_rtk. The only remaining problem we had is that we had an offset of 66.75 in the negative direction of the Z-component for whatever reason. We fixed it by adding this offset into line 4245 of the odometry.cpp file (https://github.com/ctu-mrs/mrs_uav_odometry/blob/master/src/odometry.cpp#L4245). This is maybe not the best way to do it, we plan to add an extra parameter offset and put it in the odometry config file in case this offset changes depending on the flight zone.

After applying this offset everything was fine in the X,Y and Z direction. We made it fly and sent it different commands in the 3 directions and it flew precisely and in a stable way.

Thank you very much for your help and tips.

Zakaria

[RaphaelNetels]

Hello MRS,

I was trying to tune my parameters by changing the covariance matrix as was done in this example because the drone is not super stable. Here you can find my bag files with the plots I get in the x and y plane (https://we.tl/t-5upM2aVcVt). It is possible to see the spikes on the odom_main which is weird and I want to reduce this. The only maneuvers that are done are to take off the drone and let it land.

I am not able to perform the necessary steps to tweak the covariance matrix and see the results. What I do is the following in different terminals.

• roscore
• rosbag play xxx.bag --clock
• rosparam set use_sim_time true
• roslaunch mrs_uav_odometry odometry.launch after changing the custom configs like this (custom_configs.zip) and having remapped uav_state to uav_state_new.
• rosbag record -a

Afterward, when I try to plot uav_state_new in plotjuggler I get a very noisy signal that I can not use. Am I missing a step or did I not configure the custom configs in a good way? How did you change your configs? Secondly, when plotting the z I get the same results as described above. Is there any update on this?

Thank you for your response!

Raphael

[petrlmat]

Hi, the spikes you see are cause by RTK measurements coming at a low rate (1 Hz). In contrast the GPS measurements are coming at 100 Hz. The estimate follows the trend of the GPS and every second it is abruptly corrected by the RTK measurement. To avoid such spikes, try increasing the measurement covariance R from 1 to 10. You can also try reducing max_rtk_pos_correction. This should make the RTK corrections more smooth.

Your steps seem correct. You can double check if your custom configs are actually getting loaded. Generally, if you have a noisy signal coming out of the estimator, you have noisy measurements and your measurement covariance is too low.

We are aware of the issue in RTK altitude initialization and a fix should come next week.

Also next time please open new issue (or discussion) by following the recently added templates instead of writing into an already closed issue. We are checking the open issues for new ones, but the closed ones we treat as solved.