1
$\begingroup$

I am currently estimating a robot's pose (3d position and rotation matrix) with an IMU and I want to reset the localizer in the middle of the run while keeping the relationship to the pose it had when the reset occurs.

Basically, at time t, I have the position and velocity in the world frame of the robot as well as the rotation matrix from the body-fixed frame to the world frame. If, I then reset the localizer at time t and set the position to the origin and the rotation matrix to the identity; how should the velocity be rotated such that I can relate the new estimated trajectory to the pose the vehicle had when it reset at time t by:

$$ \mathbf{p}^{world}_{t+n} = \mathbf{R}_{t}\mathbf{p}_{t+n}+\mathbf{p}_{t}$$ $$ \mathbf{R}^{world}_{t+n} = \mathbf{R}_{t}\mathbf{R}_{t+n} $$

where n is any index of the estimated positions after the reset at time t.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
1
$\begingroup$

Instead of just considering it a reset, you should consider it a coordinate system transformation:

  • You set the position to zero, that means that you create a transformation matrix which brings your current position to zero.

  • You set the rotation matrix to the identity matrix, so you create a transformation matrix, which transforms your current rotation state to zero rotation.

You can build these matrices as the inverse of the transformation matrices you already have. These matrices can then be used to convert the velocity vector (or even the whole previous trajectory) to the new frame.

Do not do a reset by attributing a new position and rotation value, find the coordinate transformation which brings the current frame to the desired frame and use that transformation to transform all elements of the original frame.

Algorithmically, instead of

newPos = defaultPos

use

newPos = getTMat(oldPos, defaultPos) * oldPos

If you have a velocity vector in the previous coordinate frame, oldVel = (xvel, yvel, zvel) and you want to convert it to the new coordinate frame:

newVel = R * oldVel

Where R is the rotation part (3x3) of the T transformation matrix (4x4) between the two frames. Please note, that if the velocity is expresses in a different frame (e.g. the mobile robots coordinates) you will need the rotation matrix between the robot and the old frame first:

newVel = R_new, old * R_old, robot * oldVel
Improve this answer
$\endgroup$
19
  • $\begingroup$ Thank you very much for your answer! The reason (I'm not entirely sure of this) for considering it as a reset is to make "Conditionally independent submaps created by EKF-SLAM". It is from a paper I am trying to replicate: Karlsson, Anders, et al. "Smoothing-based submap merging in large area SLAM." Scandinavian Conference on Image Analysis. Springer, Berlin, Heidelberg, 2011. $\endgroup$
    – Damuno
    May 18, 2020 at 6:24
  • $\begingroup$ The paper states the following: "A new submap is initiated when the sensor has moved a specified distance from the start of the current submap. Each submap is locally referenced, i.e., the sensor position is set to the origin and the orientation to a default rotation matrix. The covariance for the new pose is zero. The velocity of the sensor is initiated with the velocity of the sensor in the previous submap, rotated to compensate for resetting the sensor orientation." $\endgroup$
    – Damuno
    May 18, 2020 at 6:24
  • $\begingroup$ However, the relation between the submaps are known as the submap is initiated at the last position of the previous submap. Thus, enabling me to reference them together in the same coordinate system by the relation I stated in my question. $\endgroup$
    – Damuno
    May 18, 2020 at 6:26
  • $\begingroup$ Would you say that your approach corresponds to exactly this? $\endgroup$
    – Damuno
    May 18, 2020 at 6:28
  • $\begingroup$ Resetting a rotation/translation or finding a transformation matrix which resets the value has the same effect on the frame. The difference is that the transformation matrix can be used to transform other values (e.g velocities) between the frames. The difference in code is essentially newPos = defaultPos or newPos = getTMat(oldPos, defaultPos) * oldPos $\endgroup$
    – 50k4
    May 18, 2020 at 6:30

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.