zm0612/eskf-gps-imu-fusion

R_nm_nm_1的计算问题

hjwang1 opened this issue · 2 comments

eskf.cpp(当前主分支150行),计算R_nm_nm_1时,用angle_axisd对应的旋转矩阵的transpose,210行在使用R_nm_nm_1时,又transpose了一下,这是误操作?还是姿态解算方程使然?

zm0612 commented

我看了一下,第二个转置应该要去掉。
也就是这个转置保留:

const Eigen::Matrix3d R_nm_nm_1 = angle_axisd.toRotationMatrix().transpose();

这个转置去掉:
curr_R = R_nm_nm_1.transpose() * pose_.block<3, 3>(0, 0) * angle_axisd.toRotationMatrix();

你看一下这样改之后,是不是就是对的了。这一块内容时间有点儿久了,公式我记得不是很清晰,得翻书重新推导验证一遍。

我看了一下,第二个转置应该要去掉。 也就是这个转置保留:

const Eigen::Matrix3d R_nm_nm_1 = angle_axisd.toRotationMatrix().transpose();

这个转置去掉:

curr_R = R_nm_nm_1.transpose() * pose_.block<3, 3>(0, 0) * angle_axisd.toRotationMatrix();

你看一下这样改之后,是不是就是对的了。这一块内容时间有点儿久了,公式我记得不是很清晰,得翻书重新推导验证一遍。

感觉你这样改是对的,w_in是时刻 m-1 -> m 地球转动引起的导航系转动角速度,而姿态更新公式如果写成:
CodeCogsEqn
那么右侧第一项是地球从m->m-1的旋转矩阵,那么应该只有一个转置🤔
(我纯新手,有高手的话欢迎改正!)