fuxiaoer/KF-GINS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

explicitly normalized quaternion

Browse Source
This commit is contained in:
liqiang 2024-09-30 21:15:27 +08:00
parent 7685d69ea2
commit 67fb6a5676
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/common/earth.h
View File

@ -113,7 +113,7 @@ public:
quat.y() = sinlat * coslon; quat.y() = sinlat * coslon;
quat.z() = coslat * sinlon; quat.z() = coslat * sinlon;
return quat; return quat.normalized();
} }
/* 从n系到e系转换四元数得到纬度和经度 */ /* 从n系到e系转换四元数得到纬度和经度 */

10
src/kf-gins/insmech.cpp
View File

@ -83,7 +83,7 @@ void INSMech::velUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const
temp2 << 0, 0, -WGS84_WIE * imucur.dt / 2; temp2 << 0, 0, -WGS84_WIE * imucur.dt / 2;
qee = Rotation::rotvec2quaternion(temp2); qee = Rotation::rotvec2quaternion(temp2);
qne = Earth::qne(pvapre.pos); qne = Earth::qne(pvapre.pos);
qne = qee * qne * qnn; qne = (qee * qne * qnn).normalized();
midpos[2] = pvapre.pos[2] - midvel[2] * imucur.dt / 2; midpos[2] = pvapre.pos[2] - midvel[2] * imucur.dt / 2;
midpos = Earth::blh(qne, midpos[2]); midpos = Earth::blh(qne, midpos[2]);
@ -141,7 +141,7 @@ void INSMech::posUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const
// 位置更新完成 // 位置更新完成
// position update finish // position update finish
qne = Earth::qne(pvapre.pos); qne = Earth::qne(pvapre.pos);
qne = qee * qne * qnn; qne = (qee * qne * qnn).normalized();
pvacur.pos[2] = pvapre.pos[2] - midvel[2] * imucur.dt; pvacur.pos[2] = pvapre.pos[2] - midvel[2] * imucur.dt;
pvacur.pos = Earth::blh(qne, pvacur.pos[2]); pvacur.pos = Earth::blh(qne, pvacur.pos[2]);
} }
@ -156,8 +156,8 @@ void INSMech::attUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const
midvel = (pvapre.vel + pvacur.vel) / 2; midvel = (pvapre.vel + pvacur.vel) / 2;
qne_pre = Earth::qne(pvapre.pos); qne_pre = Earth::qne(pvapre.pos);
qne_cur = Earth::qne(pvacur.pos); qne_cur = Earth::qne(pvacur.pos);
temp1 = Rotation::quaternion2vector(qne_cur.inverse() * qne_pre); temp1 = Rotation::quaternion2vector((qne_cur.inverse() * qne_pre).normalized());
qne_mid = qne_pre * Rotation::rotvec2quaternion(temp1 / 2).inverse(); qne_mid = (qne_pre * Rotation::rotvec2quaternion(temp1 / 2).inverse()).normalized();
midpos[2] = (pvacur.pos[2] + pvapre.pos[2]) / 2; midpos[2] = (pvacur.pos[2] + pvapre.pos[2]) / 2;
midpos = Earth::blh(qne_mid, midpos[2]); midpos = Earth::blh(qne_mid, midpos[2]);
@ -183,7 +183,7 @@ void INSMech::attUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const
// 姿态更新完成 // 姿态更新完成
// attitude update finish // attitude update finish
pvacur.att.qbn = qnn * pvapre.att.qbn * qbb; pvacur.att.qbn = (qnn * pvapre.att.qbn * qbb).normalized();
pvacur.att.cbn = Rotation::quaternion2matrix(pvacur.att.qbn); pvacur.att.cbn = Rotation::quaternion2matrix(pvacur.att.qbn);
pvacur.att.euler = Rotation::matrix2euler(pvacur.att.cbn); pvacur.att.euler = Rotation::matrix2euler(pvacur.att.cbn);
} }