//
// geo_ned.cpp
// EKF
//
#include "geo_ned.hpp"
using namespace geodectic_converter;
GeodecticConverter::GeodecticConverter()
:_have_reference(false)
{}
bool GeodecticConverter::isInitialised()
{
return _have_reference;
}
void GeodecticConverter::getReference(double &latitude, double &longitude, double &altitude)
{
latitude = _initial_latitude;
longitude = _initial_longitude;
altitude = _initial_altitude;
return;
}
void GeodecticConverter::intializeReference(const double latitude, const double longitude, const double altitude)
{
// Save origin
_initial_latitude = deg2Rad(latitude);
_initial_longitude = deg2Rad(longitude);
_initial_altitude = altitude;
// Compute ECEF of NED origin
geodetic2Ecef(latitude, longitude, altitude, &_initial_ecef_x, &_initial_ecef_y, &_initial_ecef_z);
// Compute ECEF to NED and NED to ECEF matrices
double phiP = atan2(_initial_ecef_z, sqrt(pow(_initial_ecef_x, 2) + pow(_initial_ecef_y, 2)));
ecef_to_ned_matrix_ = nRe(phiP, _initial_longitude);
ned_to_ecef_matrix_ = nRe(_initial_latitude, _initial_longitude).transpose();
_have_reference = true;
}
void GeodecticConverter::geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x,
double* y, double* z)
{
// Convert geodetic coordinates to ECEF.
// http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
double lat_rad = deg2Rad(latitude);
double lon_rad = deg2Rad(longitude);
double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
*x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
*y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
*z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad);
}
void GeodecticConverter::ecef2Geodetic(const double x, const double y, const double z, double* latitude,
double* longitude, double* altitude)
{
// Convert ECEF coordinates to geodetic coordinates.
// J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
// to geodetic coordinates," IEEE Transactions on Aerospace and
// Electronic Systems, vol. 30, pp. 957-961, 1994.
double r = sqrt(x * x + y * y);
double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq;
double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+ sqrt(
0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
- P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
double V = sqrt(
pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z);
double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
*altitude = U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V));
*latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r));
*longitude = rad2Deg(atan2(y, x));
}
void GeodecticConverter::ecef2Ned(const double x, const double y, const double z, double* north, double* east,
double* down)
{
// Converts ECEF coordinate position into local-tangent-plane NED.
// Coordinates relative to given ECEF coordinate frame.
Eigen::Vector3d vect, ret;
vect(0) = x - _initial_ecef_x;
vect(1) = y - _initial_ecef_y;
vect(2) = z - _initial_ecef_z;
ret = ecef_to_ned_matrix_ * vect;
*north = ret(0);
*east = ret(1);
*down = -ret(2);
}
void GeodecticConverter::ned2Ecef(const double north, const double east, const double down, double* x, double* y,
double* z)
{
// NED (north/east/down) to ECEF coordinates
Eigen::Vector3d ned, ret;
ned(0) = north;
ned(1) = east;
ned(2) = -down;
ret = ned_to_ecef_matrix_ * ned;
*x = ret(0) + _initial_ecef_x;
*y = ret(1) + _initial_ecef_y;
*z = ret(2) + _initial_ecef_z;
}
void GeodecticConverter::geodetic2Ned(const double latitude, const double longitude, const double altitude,
double* north, double* east, double* down)
{
// Geodetic position to local NED frame
double x, y, z;
geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
ecef2Ned(x, y, z, north, east, down);
}
void GeodecticConverter::ned2Geodetic(const double north, const double east, const double down, double* latitude,
double* longitude, double* altitude)
{
// Local NED position to geodetic coordinates
double x, y, z;
ned2Ecef(north, east, down, &x, &y, &z);
ecef2Geodetic(x, y, z, latitude, longitude, altitude);
}
void GeodecticConverter::geodetic2Enu(const double latitude, const double longitude, const double altitude,
double* east, double* north, double* up)
{
// Geodetic position to local ENU frame
double x, y, z;
geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
double aux_north, aux_east, aux_down;
ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);
*east = aux_east;
*north = aux_north;
*up = -aux_down;
}
void GeodecticConverter::enu2Geodetic(const double east, const double north, const double up, double* latitude,
double* longitude, double* altitude)
{
// Local ENU position to geodetic coordinates
const double aux_north = north;
const double aux_east = east;
const double aux_down = -up;
double x, y, z;
ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
ecef2Geodetic(x, y, z, latitude, longitude, altitude);
}
没有合适的资源?快使用搜索试试~ 我知道了~
资源推荐
资源详情
资源评论
收起资源包目录
状态估计-融合GPS+IMU+编码器传感器实现的准确的状态估计-附项目源码-优质项目实战.zip (345个子文件)
Array 304B
Cholesky 775B
CholmodSupport 2KB
Core 13KB
geo_ned.cpp 6KB
fusion.cpp 5KB
run_fusion.cpp 4KB
ekf.cpp 4KB
utils.cpp 1KB
main.cpp 935B
data.csv 245KB
Dense 122B
.DS_Store 8KB
.DS_Store 6KB
.DS_Store 6KB
.DS_Store 6KB
Eigen 37B
Eigen2Support 3KB
Eigenvalues 1KB
Geometry 2KB
Eigen_Colamd.h 60KB
Transform.h 56KB
GeneralBlockPanelKernel.h 45KB
SparseMatrix.h 44KB
JacobiSVD.h 40KB
Functors.h 38KB
Memory.h 37KB
PlainObjectBase.h 35KB
BlockMethods.h 34KB
blas.h 33KB
SuperLUSupport.h 32KB
TriangularMatrix.h 30KB
SelfAdjointEigenSolver.h 30KB
CwiseNullaryOp.h 29KB
Quaternion.h 29KB
SparseLU.h 28KB
FullPivLU.h 28KB
Transform.h 28KB
GeneralProduct.h 27KB
DenseCoeffsBase.h 27KB
VectorwiseOp.h 26KB
SparseBlock.h 26KB
SparseQR.h 26KB
Macros.h 25KB
PermutationMatrix.h 25KB
PacketMath.h 24KB
MatrixBase.h 24KB
Assign.h 23KB
PaStiXSupport.h 23KB
SimplicialCholesky.h 23KB
FullPivHouseholderQR.h 23KB
GeneralMatrixVector.h 23KB
MathFunctions.h 22KB
DenseBase.h 22KB
RealQZ.h 22KB
Tridiagonalization.h 22KB
ColPivHouseholderQR.h 22KB
EigenSolver.h 21KB
LDLT.h 21KB
PardisoSupport.h 21KB
CholmodSupport.h 20KB
RealSchur.h 19KB
CoeffBasedProduct.h 19KB
HouseholderSequence.h 19KB
SparseMatrixBase.h 19KB
PacketMath.h 18KB
SVD.h 18KB
SparseSelfAdjointView.h 18KB
TriangularMatrixMatrix.h 18KB
PartialPivLU.h 18KB
Complex.h 18KB
Constants.h 17KB
Matrix.h 17KB
XprHelper.h 17KB
DenseStorage.h 17KB
Quaternion.h 17KB
ComplexSchur.h 16KB
UmfPackSupport.h 16KB
GeneralMatrixMatrix.h 16KB
Block.h 16KB
LLT.h 16KB
Amd.h 16KB
SelfadjointMatrixMatrix.h 15KB
MathFunctions.h 15KB
PacketMath.h 15KB
Transpositions.h 15KB
IncompleteLUT.h 15KB
GeneralizedEigenSolver.h 15KB
TriangularMatrixVector.h 15KB
Transpose.h 15KB
Inverse.h 14KB
Jacobi.h 14KB
HessenbergDecomposition.h 14KB
Redux.h 14KB
TriangularSolverMatrix.h 14KB
AlignedBox.h 14KB
GeneralMatrixMatrixTriangular.h 14KB
ForwardDeclarations.h 13KB
SparseVector.h 13KB
Inverse_SSE.h 13KB
共 345 条
- 1
- 2
- 3
- 4
资源评论
极智视界
- 粉丝: 2w+
- 资源: 1575
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功