The output topic /odometry/filtered fps is not stable.
The output topic /odometry/filtered fps is not stable.
The caculation of fps is 1/(stampt1 - stampt0).
The strange thing is that the rostopic hz /odometry/filtered frequency is stable.
average rate: 19.888
min: 0.050s max: 0.050s std dev: 0.00005s window: 477
average rate: 19.888
min: 0.050s max: 0.050s std dev: 0.00005s window: 497
average rate: 19.888
min: 0.050s max: 0.050s std dev: 0.00005s window: 517
average rate: 19.888
min: 0.050s max: 0.050s std dev: 0.00005s window: 537
average rate: 19.888
min: 0.050s max: 0.050s std dev: 0.00005s window: 557
average rate: 19.888
min: 0.050s max: 0.050s std dev: 0.00005s window: 577
average rate: 19.888
min: 0.050s max: 0.050s std dev: 0.00005s window: 597
average rate: 19.888
min: 0.050s max: 0.050s std dev: 0.00005s window: 605
image description x-coordicate is the number of frame. y-coordicate is the fps per frame.
my launch file:
<launch>
<arg name="output_final_position" default="true" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="rosbag" type="play" name="rosbagplay" args="/output.bag --clock -d 5" required="true"/>
<node name="test_ekf_zp" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="20"/>
<param name="sensor_timeout" value="0.05"/>
<param name="two_d_mode" value="false"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="transform_timeout" value="0.0"/>
<param name="odom0" value="/loop_fusion/odometry_rect"/>
<param name="imu0" value="/mynteye/imu/data_raw"/>
<rosparam param="odom0_config">[true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
true, true, false,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="1"/>
<param name="imu0_queue_size" value="50"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<param name="reset_on_time_jump" value="true"/>
<rosparam param="process_noise_covariance">[0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0 ...