pvapre 上一时刻状态
pvacur 输出当前时刻状态
posUpdate 位置更新
velUpdate 速度更新
attUpdate 姿态更新
#include "insmech.h"
void INSMech::insMech(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {
// perform velocity update, position updata and attitude update in sequence, irreversible order
// 依次进行速度更新、位置更新、姿态更新, 不可调换顺序
velUpdate(pvapre, pvacur, imupre, imucur);
posUpdate(pvapre, pvacur, imupre, imucur);
attUpdate(pvapre, pvacur, imupre, imucur);
}
位置——姿态——速度
// 计算地理参数,子午圈半径和卯酉圈半径,地球自转角速度投影到n系, n系相对于e系转动角速度投影到n系,重力值
// calculate geographic parameters, Meridian and Mao unitary radii,
// earth rotational angular velocity projected to n-frame,
// rotational angular velocity of n-frame to e-frame projected to n-frame, and gravity
Eigen::Vector2d rmrn = Earth::meridianPrimeVerticalRadius(pvapre.pos(0));
Eigen::Vector3d wie_n, wen_n;
wie_n << WGS84_WIE * cos(pvapre.pos[0]), 0, -WGS84_WIE * sin(pvapre.pos[0]);
wen_n << pvapre.vel[1] / (rmrn[1] + pvapre.pos[2]), -pvapre.vel[0] / (rmrn[0] + pvapre.pos[2]),
-pvapre.vel[1] * tan(pvapre.pos[0]) / (rmrn[1] + pvapre.pos[2]);
double gravity = Earth::gravity(pvapre.pos)
重力计算在earth文件中
static double gravity(const Vector3d &blh) {
double sin2 = sin(blh[0]);
sin2 *= sin2;
return 9.7803267715 * (1 + 0.0052790414 * sin2 + 0.0000232718 * sin2 * sin2) +
blh[2] * (0.0000000043977311 * sin2 - 0.0000030876910891) + 0.0000000000007211 * blh[2] * blh[2];
}
// 旋转效应和双子样划桨效应
// rotational and sculling motion
temp1 = imucur.dtheta.cross(imucur.dvel) / 2;
temp2 = imupre.dtheta.cross(imucur.dvel) / 12;
temp3 = imupre.dvel.cross(imucur.dtheta) / 12;
// b系比力积分项
// velocity increment due to the specific force
d_vfb = imucur.dvel + temp1 + temp2 + temp3;
// 比力积分项投影到n系
// velocity increment dut to the specfic force projected to the n-frame
temp1 = (wie_n + wen_n) * imucur.dt / 2;
cnn = I33 - Rotation::skewSymmetric(temp1);
d_vfn = cnn * pvapre.att.cbn * d_vfb;
// 计算重力/哥式积分项
// velocity increment due to the gravity and Coriolis force
gl << 0, 0, gravity;
d_vgn = (gl - (2 * wie_n + wen_n).cross(pvapre.vel)) * imucur.dt;
// 得到中间时刻速度
// velocity at k-1/2
midvel = pvapre.vel + (d_vfn + d_vgn) / 2;
// 外推得到中间时刻位置
// position extrapolation to k-1/2
qnn = Rotation::rotvec2quaternion(temp1);
temp2 << 0, 0, -WGS84_WIE * imucur.dt / 2;
qee = Rotation::rotvec2quaternion(temp2);
qne = Earth::qne(pvapre.pos);
qne = qee * qne * qnn;
midpos[2] = pvapre.pos[2] - midvel[2] * imucur.dt / 2;
midpos = Earth::blh(qne, midpos[2]);
这里需要从n系到e系转换四元数得到纬度和经度
static Vector3d blh(const Quaterniond &qne, double height) {
return {-2 * atan(qne.y() / qne.w()) - M_PI * 0.5, 2 * atan2(qne.z(), qne.w()), height};
}
为了得到从n系到e的四元数
其中四元数需要从旋转矢量得到
// 重新计算中间时刻的rmrn, wie_e, wen_n
// recompute rmrn, wie_n, and wen_n at k-1/2
rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
-midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);
// 重新计算n系下平均比力积分项
// recompute d_vfn
temp3 = (wie_n + wen_n) * imucur.dt / 2;
cnn = I33 - Rotation::skewSymmetric(temp3);
d_vfn = cnn * pvapre.att.cbn * d_vfb;
之前得到的比力积分项式预估得到的,其中wie_n 、wen_n是k时刻并不是k-1/2时刻。
// 重新计算重力、哥式积分项
// recompute d_vgn
gl << 0, 0, Earth::gravity(midpos);
d_vgn = (gl - (2 * wie_n + wen_n).cross(midvel)) * imucur.dt
// 速度更新完成
// velocity update finish
pvacur.vel = pvapre.vel + d_vfn + d_vgn;
}
// 重新计算中间时刻的速度和位置
// recompute velocity and position at k-1/2
midvel = (pvacur.vel + pvapre.vel) / 2;
midpos = pvapre.pos + Earth::DRi(pvapre.pos) * midvel * imucur.dt / 2;
这里进行了线性假设,存在一定的误差,不过影响应该不大。
// 重新计算中间时刻地理参数
// recompute rmrn, wie_n, wen_n at k-1/2
Eigen::Vector2d rmrn;
Eigen::Vector3d wie_n, wen_n;
rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
-midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);
与之前一样过,就不用放图了
// 重新计算 k时刻到k-1时刻 n系旋转矢量
// recompute n-frame rotation vector (n(k) with respect to n(k-1)-frame)
temp1 = (wie_n + wen_n) * imucur.dt;
qnn = Rotation::rotvec2quaternion(temp1);
// e系转动等效旋转矢量 (k-1时刻k时刻,所以取负号)
// e-frame rotation vector (e(k-1) with respect to e(k)-frame)
temp2 << 0, 0, -WGS84_WIE * imucur.dt;
qee = Rotation::rotvec2quaternion(temp2);
符号相反由于坐标系转换方向不同
// 位置更新完成
// position update finish
qne = Earth::qne(pvapre.pos);
qne = qee * qne * qnn;
pvacur.pos[2] = pvapre.pos[2] - midvel[2] * imucur.dt;
pvacur.pos = Earth::blh(qne, pvacur.pos[2]);
}
两次的位置更新方法不一样:
第一次位置更新采用讲义上的方法,是通过n系下速度计算出n系下相对位置变化,进一步转到大地坐标的相对位置进行位置更新,然而两个时刻位置不一样,n系是有略微变化的,第一次重相当忽略了这个变化。
第二次位置更新采用四元数更新,考虑了n系的变化。
Eigen::Quaterniond qne_pre, qne_cur, qne_mid, qnn, qbb;
Eigen::Vector3d temp1, midpos, midvel;
// 重新计算中间时刻的速度和位置
// recompute velocity and position at k-1/2
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();
midpos[2] = (pvacur.pos[2] + pvapre.pos[2]) / 2;
midpos = Earth::blh(qne_mid, midpos[2]);
// 重新计算中间时刻地理参数
// recompute rmrn, wie_n, wen_n at k-1/2
Eigen::Vector2d rmrn;
Eigen::Vector3d wie_n, wen_n;
rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
-midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);
// 计算n系的旋转四元数 k-1时刻到k时刻变换
// n-frame rotation vector (n(k-1) with respect to n(k)-frame)
temp1 = -(wie_n + wen_n) * imucur.dt;
qnn = Rotation::rotvec2quaternion(temp1);
// 计算b系旋转四元数 补偿二阶圆锥误差
// b-frame rotation vector (b(k) with respect to b(k-1)-frame)
// compensate the second-order coning correction term.
temp1 = imucur.dtheta + imupre.dtheta.cross(imucur.dtheta) / 12;
qbb = Rotation::rotvec2quaternion(temp1);
// 姿态更新完成
// attitude update finish
pvacur.att.qbn = qnn * pvapre.att.qbn * qbb;
pvacur.att.cbn = Rotation::quaternion2matrix(pvacur.att.qbn);
pvacur.att.euler = Rotation::matrix2euler(pvacur.att.cbn);
}
感谢武汉大学卫星导航定位技术研究中心多源智能导航实验室(i2Nav)牛小骥教授团队开源的KF-GINS软件平台。