From 67fb6a567677f27e7a5c46adfd4b2a40a23bed3e Mon Sep 17 00:00:00 2001 From: liqiang Date: Mon, 30 Sep 2024 21:15:27 +0800 Subject: [PATCH] explicitly normalized quaternion --- src/common/earth.h | 2 +- src/kf-gins/insmech.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/common/earth.h b/src/common/earth.h index ed37525..ddab321 100644 --- a/src/common/earth.h +++ b/src/common/earth.h @@ -113,7 +113,7 @@ public: quat.y() = sinlat * coslon; quat.z() = coslat * sinlon; - return quat; + return quat.normalized(); } /* 从n系到e系转换四元数得到纬度和经度 */ diff --git a/src/kf-gins/insmech.cpp b/src/kf-gins/insmech.cpp index 7740730..e8b7b1d 100644 --- a/src/kf-gins/insmech.cpp +++ b/src/kf-gins/insmech.cpp @@ -83,7 +83,7 @@ void INSMech::velUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const temp2 << 0, 0, -WGS84_WIE * imucur.dt / 2; qee = Rotation::rotvec2quaternion(temp2); 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 = Earth::blh(qne, midpos[2]); @@ -141,7 +141,7 @@ void INSMech::posUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const // 位置更新完成 // position update finish 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 = 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; qne_pre = Earth::qne(pvapre.pos); qne_cur = Earth::qne(pvacur.pos); - temp1 = Rotation::quaternion2vector(qne_cur.inverse() * qne_pre); - qne_mid = qne_pre * Rotation::rotvec2quaternion(temp1 / 2).inverse(); + temp1 = Rotation::quaternion2vector((qne_cur.inverse() * qne_pre).normalized()); + qne_mid = (qne_pre * Rotation::rotvec2quaternion(temp1 / 2).inverse()).normalized(); midpos[2] = (pvacur.pos[2] + pvapre.pos[2]) / 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 - 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.euler = Rotation::matrix2euler(pvacur.att.cbn); } \ No newline at end of file