IMU and Sensor Coordinates confusion

[ghost]

Hello
im working on the asctec pelican for ly thesis
im trying to implement the ethzasl_sensor_fusion in the pelican . I have gone through the tutorial
http://wiki.ros.org/ethzasl_sensor_fusion/Tutorials/getting_started
From the asctec website the coordinate frames of the pelican IMU are

Which give the configuration as follows

Im running ethzasl_ptam and rostopic echo/vslam/pose_world gives

where as rostopic echo /fcu/imu gives

my imu Z axis seems to be parallel to the ptam's Z axis
the ptams Y_axis is parallel to the above cameras Z_Axis(pointing out the lens)
the ptams X_axis is perpendicular to the above cameras Z_Axis(pointing out the lens)

I have read all the previous discussions but still unable to deduce how can i find

how can in compute the q_ci, . form this information?
How to compute the quaternions p_ci and q_wv ?

I am sorry if my questions r too intuitaive / basic.

Thanks
Farhan

[stephanweiss]

Farhan,

for q_wv:
if you initialize PTAM over a horizontal plane then q_wv is the unit quaternion. Use this setup first to eliminate any error sources coming from a potentially wrong q_wv. In case the PTAM map (i.e. the grid that is displayed in the remote_ptam view or PTAM gui) is inclined you need to find the azimuth and elevation angle with respect to the gravity vector (e.g. by using the accelerometer reading in hover mode and the current PTAM attitude measurement).

for q_ci:
in the above image, the device you denoted as IMU is in fact the magnetometer. The axis of the pelican are different: x is "foreward" (i.e. your x_imu I think), y is then to the left, and z is up. To verify, place the UAV such that the accelerometer only ready +9.81 on one single axis (zero on the other two), the side of the UAV that is pointing "up" in this configuration is then the positive direction of the axis that shows +9.81. If I am not mistaken, the cameras coordinate system is as follows: +x is towards the small multi-pin connector, +y is towards the mini USB connector and +z is out of the lens. With this you can find the transformation between IMU and cam.

If all fails you might want to try the following:

• initialize q_ci with w=0, x=0.9239, y=-0.3827, z=0 (just a 5sec guess of mine)
• initialize q_wv with the unit quaternion
  (- initialize p_ic with [0 0 0] )
• initialize PTAM over a horizontal plane with very good features such that you have a very robust map
• augment the noise_qci value in the pose_filter dynamic reconfigure GUI to about 0.1
• init the filter
• excite the UAV in angular rotation as much as motion blur and camera FoV on the map allows it and plot the evolution of the q_ci state.
• stop as soon as q_ci converged, read out the values of q_ci and use them as init values in the future

hope that helps.

best,
Stephan

[ghost]

Thanks Stephan

I am following the tutorial to set up SFlay and connected the SFly Network as follows

The problem is in the fcu_parameter.launch file
when i set fcu/packet rate ekf state : 100
i get a lot of checksum erros with the mentioned baud rate of 460800

and the pose filter diplays

but when in decrease packet rate ekf to 10 to 20 its okay

kindly suggest if i need to reconfigure the serial port as defined in the master mind bringup ros page (which i followed). or need to decrease some other parameters.

Thanks
Farhan

[stephanweiss]

Farhan, please try a lower baud rate first (e.g. 115200 should be sufficient for 100Hz transmission). Also set all other topic transmissions to 1 or 0Hz (in the dynamic reconfigure in the fcu node)

Let me know if that works

[ghost]

Hi Stephan
I managed to modify the fcu_parameters.launch file as
fcu/serial_port: /dev/ttyS2
fcu/baudrate: 115200
fcu/frame_id: fcu
fcu/packet_rate_imu: 0.0
fcu/packet_rate_rc: 0.0
fcu/packet_rate_gps: 0.0
fcu/packet_rate_ssdk_debug: 0.0
fcu/packet_rate_ekf_state: 100.0
fcu/packet_rate_msg: 1.0
fcu/max_velocity_xy: 2.0
fcu/max_velocity_z: 2.0

fcu/state_estimation: HighLevel_EKF
fcu/position_control: HighLevel
fcu/battery_warning: 10.8

This time i get checksum errors at a low frequency then before like after 5 to 10 seconds
Q1. how can i eliminate them?
My rqt_graph now look like

iv initialized the pose sensor and im getting values on /ssf_core/pose
Q2 iv initialised lemda = PTAM(hight)/hight(matric). is PTAM height the value of Z position /vslam/pose/pose/position/z which is in meters? and hight(matric) is the measured heght form the gournd (since pelican is on the ground )to the pelican IMU(autopilot) or to the camera?

Q3. how can i plot the evolution of the q_ci state ?

Thanks
Regards
Farhan

[ghost]

Hi Stephan
rostopic echo /fcu/ekf_state_in gives all the values as zeros but i have covariences, is this okay?
just for reference pose_sensor_launch looks like

        <remap from="ssf_core/pose_measurement" to="/vslam/pose" />
         <remap from="ssf_core/hl_state_input" to="/fcu/ekf_state_out" /> <!-- reads IMU data and predicted state from HLP -->
    <remap from="ssf_core/correction" to="/fcu/ekf_state_in" /> <!-- sends the correction values to the HLP state perdiction --> 

    <rosparam file="$(find ssf_updates)/pose_sensor_fix.yaml"/>
</node>

and pose_sensor_fix.yaml is
scale_init: 1
fixed_scale: 0
fixed_bias: 0
noise_acc: 0.083
noise_accbias: 0.0083
noise_gyr: 0.0013
noise_gyrbias: 0.00013
noise_scale: 0.0
noise_qwv: 0.0
noise_qci: 0.0
noise_pic: 0.0
delay: 0.02
meas_noise1: 0.01
meas_noise2: 0.02

data_playback: False

initialization of camera w.r.t. IMU

init/q_ci/w: 0.0
init/q_ci/x: 0.9239
init/q_ci/y: 0.3827
init/q_ci/z: 0.0

init/p_ci/x: 0.0
init/p_ci/y: 0.0
init/p_ci/z: 0.0

initialization of world w.r.t. vision

init/q_wv/w: 1.0
init/q_wv/x: 0.0
init/q_wv/y: 0.0
init/q_wv/z: 0.0

use_fixed_covariance: False
measurement_world_sensor: False # selects if sensor measures its position w.r.t. world (true, e.g. Vicon) or the position of the world coordinate system w.r.t. the sensor (false, e.g. ethzasl_ptam)

i hope im not remapping wrong topics. !!

Thanks
Farhan

[ghost]

Hi stephan
thanks for your help !
I have managed to remove the checksum errors by restoring the ubunto mastermind recovery image and configuring the fcu_parametes as

fcu/serial_port: /dev/ttyS2
fcu/baudrate: 115200
fcu/frame_id: fcu
fcu/packet_rate_imu: 5.0
fcu/packet_rate_rc: 1.0
fcu/packet_rate_gps: 0.0
fcu/packet_rate_ssdk_debug: 1.0
fcu/packet_rate_ekf_state: 100.0
fcu/packet_rate_mag: 50.0
fcu/max_velocity_xy: 2.0
fcu/max_velocity_z: 2.0
fcu/state_estimation: HighLevel_EKF
fcu/position_control: HighLevel
fcu/battery_warning: 10.8

ans i have configured the pose_sensor_fix.ymal as

initialization of camera w.r.t. IMU

init/q_ci/w: 1.0
init/q_ci/x: 0.9329
init/q_ci/y: 0.3827
init/q_ci/z: 0.0

init/p_ci/x: 0.0
init/p_ci/y: 0.0
init/p_ci/z: 0.0

initialization of world w.r.t. vision

init/q_wv/w: 0.0
init/q_wv/x: 0.0
init/q_wv/y: 0.0
init/q_wv/z: 1.0

and remapped the topic as pose_sensor.launch as

    <remap from="ssf_core/pose_measurement" to="/vslam/pose" />

    <rosparam file="$(find ssf_updates)/pose_sensor_fix.yaml"/>

Now my questions are
Q1 iv initialised lemda = PTAM(hight)/hight(matric). is PTAM height the value of Z position /vslam/pose/pose/pose/position/z which is in meters? and hight(matric) is the measured heght form the gournd (since pelican is on the ground )to the pelican IMU(autopilot) or to the camera?
with lemda = 1 // i get fuzzy tracking

Q2 How can i plot the evolution of q_ci state ?

Q3 do i have to remap /ssf_core/pose to /fcu/pose for the Luemberger oberver?
My qrt_graph is the same as above
Thanks
Farhan

[simonlynen]

@iamfarhan007 here are some answers:

1. yes, scale \lambda is the z-position of ptam divided by the z-position of the IMU. You should only get fuzzy tracking warnings if your camera-to-imu calibration is wrong of the ptam tracking is unstable.
2. the estimator outputs the state on a separate topic. You can see the existing plotting scripts how to access elements from this vector for plotting.
3. This should be described in the tutorial.