Skip to content

Commit

Permalink
format code.
Browse files Browse the repository at this point in the history
  • Loading branch information
@YibinWu
YibinWu committed Oct 9, 2023
1 parent 8fd26fa commit 4df3599
Show file tree
Hide file tree
Showing 15 changed files with 759 additions and 858 deletions.
194 changes: 86 additions & 108 deletions src/common/rotation.h
View file
Original file line number Diff line number Diff line change
@@ -1,27 +1,4 @@
/*
* OB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System
*
* Copyright (C) 2022 i2Nav Group, Wuhan University
*
* Author : Hailiang Tang
* Contact : thl@whu.edu.cn
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/

#ifndef ROTATION_H
#define ROTATION_H
#pragma once

#include <Eigen/Geometry>

Expand All @@ -30,91 +7,92 @@ using Eigen::Quaterniond;
using Eigen::Vector3d;

class Rotation {

public:
static Quaterniond matrix2quaternion(const Matrix3d &matrix) {
return Quaterniond(matrix);
}

static Matrix3d quaternion2matrix(const Quaterniond &quaternion) {
return quaternion.toRotationMatrix();
}

static Vector3d matrix2euler(const Eigen::Matrix3d &dcm) {
Vector3d euler;

euler[1] = atan(-dcm(2, 0) / sqrt(dcm(2, 1) * dcm(2, 1) + dcm(2, 2) * dcm(2, 2)));

if (dcm(2, 0) <= -0.999) {
euler[0] = atan2(dcm(2, 1), dcm(2, 2));
euler[2] = atan2((dcm(1, 2) - dcm(0, 1)), (dcm(0, 2) + dcm(1, 1)));
} else if (dcm(2, 0) >= 0.999) {
euler[0] = atan2(dcm(2, 1), dcm(2, 2));
euler[2] = M_PI + atan2((dcm(1, 2) + dcm(0, 1)), (dcm(0, 2) - dcm(1, 1)));
} else {
euler[0] = atan2(dcm(2, 1), dcm(2, 2));
euler[2] = atan2(dcm(1, 0), dcm(0, 0));
}

// heading 0~2PI
if (euler[2] < 0) {
euler[2] = M_PI * 2 + euler[2];
}

return euler;
}

static Vector3d quaternion2euler(const Quaterniond &quaternion) {
return matrix2euler(quaternion.toRotationMatrix());
}

static Quaterniond rotvec2quaternion(const Vector3d &rotvec) {
double angle = rotvec.norm();
Vector3d vec = rotvec.normalized();
return Quaterniond(Eigen::AngleAxisd(angle, vec));
}

static Vector3d quaternion2vector(const Quaterniond &quaternion) {
Eigen::AngleAxisd axisd(quaternion);
return axisd.angle() * axisd.axis();
}

// RPY --> C_b^n, ZYX顺序
static Matrix3d euler2matrix(const Vector3d &euler) {
return Matrix3d(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *
Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}

static Quaterniond euler2quaternion(const Vector3d &euler) {
return Quaterniond(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *
Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}

static Matrix3d skewSymmetric(const Vector3d &vector) {
Matrix3d mat;
mat << 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1), vector(0), 0;
return mat;
}

static Eigen::Matrix4d quaternionleft(const Quaterniond &q) {
Eigen::Matrix4d ans;
ans(0, 0) = q.w();
ans.block<1, 3>(0, 1) = -q.vec().transpose();
ans.block<3, 1>(1, 0) = q.vec();
ans.block<3, 3>(1, 1) = q.w() * Eigen::Matrix3d::Identity() + skewSymmetric(q.vec());
return ans;
public:
static Quaterniond matrix2quaternion(const Matrix3d &matrix) {
return Quaterniond(matrix);
}

static Matrix3d quaternion2matrix(const Quaterniond &quaternion) {
return quaternion.toRotationMatrix();
}

static Vector3d matrix2euler(const Eigen::Matrix3d &dcm) {
Vector3d euler;

euler[1] =
atan(-dcm(2, 0) / sqrt(dcm(2, 1) * dcm(2, 1) + dcm(2, 2) * dcm(2, 2)));

if (dcm(2, 0) <= -0.999) {
euler[0] = atan2(dcm(2, 1), dcm(2, 2));
euler[2] = atan2((dcm(1, 2) - dcm(0, 1)), (dcm(0, 2) + dcm(1, 1)));
} else if (dcm(2, 0) >= 0.999) {
euler[0] = atan2(dcm(2, 1), dcm(2, 2));
euler[2] = M_PI + atan2((dcm(1, 2) + dcm(0, 1)), (dcm(0, 2) - dcm(1, 1)));
} else {
euler[0] = atan2(dcm(2, 1), dcm(2, 2));
euler[2] = atan2(dcm(1, 0), dcm(0, 0));
}

static Eigen::Matrix4d quaternionright(const Quaterniond &p) {
Eigen::Matrix4d ans;
ans(0, 0) = p.w();
ans.block<1, 3>(0, 1) = -p.vec().transpose();
ans.block<3, 1>(1, 0) = p.vec();
ans.block<3, 3>(1, 1) = p.w() * Eigen::Matrix3d::Identity() - skewSymmetric(p.vec());
return ans;
// heading 0~2PI
if (euler[2] < 0) {
euler[2] = M_PI * 2 + euler[2];
}
};

#endif // ROTATION_H
return euler;
}

static Vector3d quaternion2euler(const Quaterniond &quaternion) {
return matrix2euler(quaternion.toRotationMatrix());
}

static Quaterniond rotvec2quaternion(const Vector3d &rotvec) {
double angle = rotvec.norm();
Vector3d vec = rotvec.normalized();
return Quaterniond(Eigen::AngleAxisd(angle, vec));
}

static Vector3d quaternion2vector(const Quaterniond &quaternion) {
Eigen::AngleAxisd axisd(quaternion);
return axisd.angle() * axisd.axis();
}

// RPY --> C_b^n, ZYX
static Matrix3d euler2matrix(const Vector3d &euler) {
return Matrix3d(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *
Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}

static Quaterniond euler2quaternion(const Vector3d &euler) {
return Quaterniond(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *
Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}

static Matrix3d skewSymmetric(const Vector3d &vector) {
Matrix3d mat;
mat << 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1),
vector(0), 0;
return mat;
}

static Eigen::Matrix4d quaternionleft(const Quaterniond &q) {
Eigen::Matrix4d ans;
ans(0, 0) = q.w();
ans.block<1, 3>(0, 1) = -q.vec().transpose();
ans.block<3, 1>(1, 0) = q.vec();
ans.block<3, 3>(1, 1) =
q.w() * Eigen::Matrix3d::Identity() + skewSymmetric(q.vec());
return ans;
}

static Eigen::Matrix4d quaternionright(const Quaterniond &p) {
Eigen::Matrix4d ans;
ans(0, 0) = p.w();
ans.block<1, 3>(0, 1) = -p.vec().transpose();
ans.block<3, 1>(1, 0) = p.vec();
ans.block<3, 3>(1, 1) =
p.w() * Eigen::Matrix3d::Identity() - skewSymmetric(p.vec());
return ans;
}
};
116 changes: 43 additions & 73 deletions src/common/types.h
View file
Original file line number Diff line number Diff line change
@@ -1,36 +1,14 @@
/*
* OB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System
*
* Copyright (C) 2022 i2Nav Group, Wuhan University
*
* Author : Hailiang Tang
* Contact : thl@whu.edu.cn
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/

#ifndef TYPES_H
#define TYPES_H
#pragma once

#include <Eigen/Geometry>
#include <math.h>

#include <Eigen/Geometry>

const double D2R = (M_PI / 180.0);
const double R2D = (180.0 / M_PI);

const int IMU_RATE = 200;
const int RANK = 21;
const int RANK = 21;
const int NOISERANK = 18;

static constexpr double NormG = 9.782940329221166;
Expand All @@ -40,78 +18,70 @@ using Eigen::Quaterniond;
using Eigen::Vector3d;

typedef struct IMU {
double timestamp;
double dt;
double timestamp;
double dt;

Vector3d angular_velocity;
Vector3d acceleration;
Vector3d angular_velocity;
Vector3d acceleration;

} IMU;

typedef struct Attitude {
Eigen::Quaterniond qbn;
Eigen::Matrix3d cbn;
Eigen::Vector3d euler;
Eigen::Quaterniond qbn;
Eigen::Matrix3d cbn;
Eigen::Vector3d euler;
} Attitude;

typedef struct PVA {
Eigen::Vector3d pos;
Eigen::Vector3d vel;
Attitude att;
Eigen::Vector3d pos;
Eigen::Vector3d vel;
Attitude att;
} PVA;

typedef struct ImuError {
Eigen::Vector3d gyrbias;
Eigen::Vector3d accbias;
Eigen::Vector3d gyrscale;
Eigen::Vector3d accscale;
Eigen::Vector3d gyrbias;
Eigen::Vector3d accbias;
Eigen::Vector3d gyrscale;
Eigen::Vector3d accscale;
} ImuError;

typedef struct NavState {
Eigen::Vector3d pos;
Eigen::Vector3d vel;
Eigen::Vector3d euler;
Eigen::Vector3d pos;
Eigen::Vector3d vel;
Eigen::Vector3d euler;

ImuError imuerror;
ImuError imuerror;
} NavState;

typedef struct ImuNoise {
Eigen::Vector3d gyr_arw;
Eigen::Vector3d acc_vrw;
Eigen::Vector3d gyrbias_std;
Eigen::Vector3d accbias_std;
Eigen::Vector3d gyrscale_std;
Eigen::Vector3d accscale_std;
double corr_time;
Eigen::Vector3d gyr_arw;
Eigen::Vector3d acc_vrw;
Eigen::Vector3d gyrbias_std;
Eigen::Vector3d accbias_std;
Eigen::Vector3d gyrscale_std;
Eigen::Vector3d accscale_std;
double corr_time;
} ImuNoise;

typedef struct Paras {
// initial state and state standard deviation
NavState initstate;
NavState initstate_std;

// 初始状态和状态标准差
// initial state and state standard deviation
NavState initstate;
NavState initstate_std;

// IMU噪声参数
// imu noise parameters
ImuNoise imunoise;

// 安装参数
// install parameters
Eigen::Vector3d mountAngle = {0, 0, 0};

Eigen::Vector3d leverArm = {0, 0, 0};
Eigen::Vector3d odo_measurement_std = {0, 0, 0};
// imu noise parameters
ImuNoise imunoise;

// install parameters
Eigen::Vector3d mountAngle = {0, 0, 0};

double odo_update_interval;
double wheelradius;

bool ifCompensateVelocity;
Eigen::Vector3d leverArm = {0, 0, 0};
Eigen::Vector3d odo_measurement_std = {0, 0, 0};

double starttime;
double odo_update_interval;
double wheelradius;

bool ifCompensateVelocity;

} Paras;
double starttime;

#endif // TYPES_H
} Paras;
Loading

0 comments on commit 4df3599

Please sign in to comment.