mathematical derivation of "initialize_with_imu()".

[suho0515]

Hello sir,
I got a question for "initialize_with_imu()" function.
I've looked for your Documents or Papers but couldn't get a clue.

I wanna see the mathematical derivation of codes below.
`
// Get z axis, which alines with -g (z_in_G=0,0,1)
Eigen::Vector3d z_axis = linavg/linavg.norm();

// Create an x_axis
Eigen::Vector3d e_1(1,0,0);

// Make x_axis perpendicular to z
Eigen::Vector3d x_axis = e_1-z_axis*z_axis.transpose()*e_1;
x_axis= x_axis/x_axis.norm();

// Get z from the cross product of these two
Eigen::Matrix<double,3,1> y_axis = skew_x(z_axis)*x_axis;

// From these axes get rotation
Eigen::Matrix<double,3,3> Ro;
Ro.block(0,0,3,1) = x_axis;
Ro.block(0,1,3,1) = y_axis;
Ro.block(0,2,3,1) = z_axis;

`

i guess this codes initialized rotation with acceleration data of IMU.

1. z_axis ( just guessing )
   getting normalized vector and set it to "z_axis".

2. x_axis ( don't understand )
   what method have you used?

3. y_axis ( understand it )
   getting "y_axis" with multiplying two vectors using skew_symmetric matrix.

is there any reference to look at?

and thank you again like allways for your wonderful works.

[WoosikLee2510]

@suho0515

1. Assuming the imu is standing still and relatively small bias, we can compute the gravity axis by average.
2. vector rejection: https://en.wikipedia.org/wiki/Vector_projection#Vector_rejection.
3. cross-product two vectors yield an orthogonal vector of two: https://en.wikipedia.org/wiki/Cross_product#Alternative_ways_to_compute_the_cross_product

[suho0515]

@suho0515

1. Assuming the imu is standing still and relatively small bias, we can compute the gravity axis by average.
2. vector rejection: https://en.wikipedia.org/wiki/Vector_projection#Vector_rejection.
3. cross-product two vectors yield an orthogonal vector of two: https://en.wikipedia.org/wiki/Cross_product#Alternative_ways_to_compute_the_cross_product

Really appreciate for the infomation.
i totally understand "1" and "3".

I ask you one more question about "2".

I see that "vector rejection" is

but we use "b" as "unit vector (e1=(1,0,0))", so the equation is

a2 = a - (a dot b)b

i'm not sure how to match the variables.
i assume it looks like below picture.

but the code saying like
"b" is "z_axis",
"a" is "e_1".

Eigen::Vector3d x_axis = e_1-z_axis*z_axis.transpose()*e_1;

i thought
"b" is "e_1",
"a" is "z_axis".

am i missing something?

[yangyulin]

Just added some comments. 1) For z axis, Yes, we are taking average of a few raw acc readings and use that direction as z . 2) When finding x, we are actually using the projection property of vectors: z^T * (I - z*z^T) = 0, hence: z^T * (I-z*z^T)*e_1 = 0, we take: x = (I - z*z^T)*e_1. Actually, this is not optimal algorithm, because when z = e1, x will = 0. Hence, you need to be aware of this singular case. 3) when you have x and z, you can use cross product to find y.

…

On Mon, Jun 22, 2020 at 10:46 AM Woosik Lee ***@***.***> wrote: 1. Assuming the imu is standing still and relatively small bias, we can compute the gravity axis by average. 2. vector rejection: https://en.wikipedia.org/wiki/Vector_projection#Vector_rejection. 3. cross-product two vectors yield an orthogonal vector of two: https://en.wikipedia.org/wiki/Cross_product#Alternative_ways_to_compute_the_cross_product — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#74 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ADJAQCAFDO3UETJRMEWVEV3RX5VFXANCNFSM4OELAWRQ> .

-- With Best Regards, Linde Yang / Yulin Yang / 杨玉林 *UNIVERSITY of DELAWARE* PhD Candidate of Mechanical Engineering 328 Spencer Lab, Newark, DE, USA, 19716-3140 Mobile: +(01) 302-690-3926 mailto: <yyl2038@163.com> yuyang@udel.edu Web: http://udel.edu/~yuyang/ <http://udel.edu/~yuyang/>

[suho0515]

Just added some comments. 1) For z axis, Yes, we are taking average of a few raw acc readings and use that direction as z . 2) When finding x, we are actually using the projection property of vectors: z^T * (I - zz^T) = 0, hence: z^T * (I-zz^T)e_1 = 0, we take: x = (I - zz^T)e_1. Actually, this is not optimal algorithm, because when z = e1, x will = 0. Hence, you need to be aware of this singular case. 3) when you have x and z, you can use cross product to find y.
…
On Mon, Jun 22, 2020 at 10:46 AM Woosik Lee @.**> wrote: 1. Assuming the imu is standing still and relatively small bias, we can compute the gravity axis by average. 2. vector rejection: https://en.wikipedia.org/wiki/Vector_projection#Vector_rejection. 3. cross-product two vectors yield an orthogonal vector of two: https://en.wikipedia.org/wiki/Cross_product#Alternative_ways_to_compute_the_cross_product — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#74 (comment)>, or unsubscribe https://github.com/notifications/unsubscribe-auth/ADJAQCAFDO3UETJRMEWVEV3RX5VFXANCNFSM4OELAWRQ .
-- With Best Regards, Linde Yang / Yulin Yang / 杨玉林 UNIVERSITY of DELAWARE PhD Candidate of Mechanical Engineering 328 Spencer Lab, Newark, DE, USA, 19716-3140 Mobile: +(01) 302-690-3926 mailto: yyl2038@163.com yuyang@udel.edu Web: http://udel.edu/~yuyang/ http://udel.edu/~yuyang/

thank you very much!
it was very helpful.

then... are these derivation right approach?

z^T = z^T
z^T - z^T = 0
z^T - z^T * z * z^T = 0
z^T * (I-z * z^T) = 0
z^T * (I-z * z^T) * e_1 = 0

multiply "z" both side
(I-z*z^T)*e_1 = 0

if we assume that "θ = 90°" between "z" and "e_1", then projected "x" would be "0". so,
(I-z*z^T)*e_1 = x

[yangyulin]

@suho0515, For your derivation: z^T = z^T z^T - z^T = 0 z^T - z^T*z*z^T = 0 z^T * (I-z*z^T) = 0 z^T * (I-z*z^T)*e_1 = 0 *up to here, everything is right. * *Comments: The high level idea is to find a non-zero vector x that z^T*x = 0. If so, z and x are perpendicular vectors. From the above equation: z^T * (I - zz^T)*e_1 = 0, it is easy to see that: x can be chosen as (I-zz^T)*e_1, that is why we use x = (I-zz^T)*e_1. Of course, this is just one method of finding x.* *But this will not 100% work, because if z = e1, then x =0. In this case, x is a zero vector, which is not our intention. (Because if x is zero, you cannot use x to initialize the system initial rotation). * multiply "z" both side (I-z*z^T)*e_1 = 0 *To here, I guess you are doing: z * z^T * (I - z*z^T)*e_1 = 0 Then, you get: (I-z*z^T)*e_1 = 0 But you cannot do this because z*z^T is not identity. (z^T*z = 1)*

[suho0515]

@suho0515, For your derivation: z^T = z^T z^T - z^T = 0 z^T - z^Tzz^T = 0 z^T * (I-zz^T) = 0 z^T * (I-zz^T)*e_1 = 0 *up to here, everything is right. * Comments: The high level idea is to find a non-zero vector x that z^Tx = 0. If so, z and x are perpendicular vectors. From the above equation: z^T * (I - zz^T)*e_1 = 0, it is easy to see that: x can be chosen as (I-zz^T)e_1, that is why we use x = (I-zz^T)e_1. Of course, this is just one method of finding x. But this will not 100% work, because if z = e1, then x =0. In this case, x is a zero vector, which is not our intention. (Because if x is zero, you cannot use x to initialize the system initial rotation). * multiply "z" both side (I-zz^T)e_1 = 0 To here, I guess you are doing: z * z^T * (I - zz^T)e_1 = 0 Then, you get: (I-zz^T)e_1 = 0 But you cannot do this because zz^T is not identity. (z^Tz = 1)

Thank you very much!
totally understand!

multiply "z" both side
(I-z*z^T)*e_1 = 0
i thought that this part is weird. but now i get it!

Thank you again to solve out this issue!
i'll close this issue then :)