GSS data in vehicle coordinate frame

[davidoort]

#161 added a GSS to the simulator. The assumption was made that the lateral velocity was 0 so that a trivial conversion between global velocities and body longitudinal velocity could be made (by just taking the norm). As shown in a real Kistler plot here, lateral velocities are certainly smaller than longitudinal velocities but are not negligible (in this bag they reach magnitudes of ~1m/s). These velocities can significantly impact the quality of state estimates and controllers.

[SijmenHuizenga]

At this moment the GSS is perfectly usable, therefore marking as low-prio.

Before we implement this, we need a way to model the lateral velocity. Does anyone know how this could be done?

[wahllca]

It's very easy to implement, because you already have all necessary information. Right know the /fsds/testing_only/odom massage, which you also use for the GSS, contains already the lateral velocity but only in the global coordinate system.
To transfer it in the local system of the car you can simply use a transformation,

where alpha is the global orientation z in rad. Just replace the vector [x,y] with [v_x, v_y]. Depending on your CoS the sign in the sin(alpha) terme may vary.
I would appreciate if you could implement it that way or leave it in the global system, because we would revert the transformation for slam and control anyway and we are already testing with this verison (Test vesion GSS) where the topic for GSS contains the velocity vector in the global frame.

[SijmenHuizenga]

Sounds good! Do you want to make a pr, proposing a change to this file/line?

[wahllca]

Sure, I'll take a look at it.

[SijmenHuizenga]

Hi @wahllca , we are planning to release the final version for competition tonight. Were you able to take a look at this?

[wahllca]

Hey @SijmenHuizenga , sorry I couldn't make it. I am currently very busy with exams and I didn't have time to test it properly.
But because I think it is important that everybody gets the complete speed information, I would suggest to simply publish the global speed vector.
Since the Imu contains the orientation, each team can perform the transformation itself if required.
Line 30 would then look like this again:
output.linear_velocity = Vector3r(ground_truth.kinematics->twist.linear.x(), ground_truth.kinematics->twist.linear.y(), ground_truth.kinematics->twist.linear.z());

If you plan to use the vehicle coordinate system in the future you can use the quaternion ground_truth.kinematics->pose.orientation.
To calculate the local velocity vector you have to multiply the transposed matrix R by the global velocity vector.

Considering the remaining time, is it ok for you to use the global velocity vector in the GSS?

[wahllca]

@SijmenHuizenga I created a pr #186 in which I changed the velocity back to the global coordinate system.
I would like to do the transformation into vehicle system after the competition

[SijmenHuizenga]

Thank you! I will check and merge it before release 👍

Amazing to hear you want to help and continue working on this this after comp <3

[SijmenHuizenga]

Hi @wahllca, do you still want to take a look at the gss and make it output data in the vehicle coordinate frame?

[wouter-heerwegh]

Coordinate system changed from global to local car frame in #304