基于C++的GPS INS组合导航程序

基于C++的GPS/INS（捷联惯性导航系统）组合导航程序，包含关键算法实现：

头文件定义 (GNSS_INS.h)

#ifndef GNSS_INS_H
#define GNSS_INS_H

#include <vector>
#include <array>
#include <Eigen/Dense>

class GNSSINS {
   
public:
    struct IMUData {
   
        double timestamp;
        Eigen::Vector3d gyro;      // 角速度 (rad/s)
        Eigen::Vector3d accel;     // 加速度 (m/s²)
    };

    struct GNSSData {
   
        double timestamp;
        Eigen::Vector3d lla;       // 经纬高 (rad, rad, m)
        Eigen::Vector3d velocity;  // NED速度 (m/s)
        double pos_std;           // 位置标准差
        double vel_std;           // 速度标准差
    };

    struct NavigationState {
   
        Eigen::Vector3d position;  // 位置 (LLA)
        Eigen::Vector3d velocity;  // 速度 (NED)
        Eigen::Matrix3d attitude;  // 姿态矩阵 (C_b^n)
        Eigen::Vector3d bias_gyro; // 陀螺零偏
        Eigen::Vector3d bias_accel;// 加速度计零偏
    };

    GNSSINS();

    // 初始化导航系统
    bool Initialize(const GNSSData& init_gnss);

    // IMU数据更新
    void UpdateIMU(const IMUData& imu);

    // GNSS数据更新
    void UpdateGNSS(const GNSSData& gnss);

    // 获取当前导航状态
    NavigationState GetState() const;

private:
    // 状态向量: [位置(3), 速度(3), 姿态误差(3), 陀螺零偏(3), 加速度零偏(3)]
    static const int STATE_DIM = 15;
    using StateVector = Eigen::Matrix<double, STATE_DIM, 1>;
    using StateCovariance = Eigen::Matrix<double, STATE_DIM, STATE_DIM>;

    NavigationState state_;
    StateVector x_;
    StateCovariance P_;

    IMUData last_imu_;
    bool initialized_;
    double last_update_time_;

    // 地球参数
    const double earth_a = 6378137.0;        // 地球长半轴
    const double earth_f = 1.0/298.257223563; // 扁率
    const double earth_omega = 7.292115e-5;  // 地球自转角速度

    // 噪声参数
    Eigen::Matrix3d Q_gyro_;
    Eigen::Matrix3d Q_accel_;
    Eigen::Matrix3d R_gnss_pos_;
    Eigen::Matrix3d R_gnss_vel_;

    // 核心算法函数
    void Predict(const IMUData& imu);
    void Correct(const GNSSData& gnss);
    Eigen::Matrix3d SkewSymmetric(const Eigen::Vector3d& v);
    void UpdateAttitude(const Eigen::Vector3d& gyro, double dt);
    Eigen::Vector3d LLAToECEF(const Eigen::Vector3d& lla);
    Eigen::Vector3d ECEFToLLA(const Eigen::Vector3d& ecef);
};

#endif

主要实现文件 (GNSS_INS.cpp)

#include "GNSS_INS.h"
#include <iostream>
#include <cmath>

GNSSINS::GNSSINS() : initialized_(false), last_update_time_(0.0) {
   
    // 初始化噪声协方差
    Q_gyro_ = Eigen::Matrix3d::Identity() * 1e-6;
    Q_accel_ = Eigen::Matrix3d::Identity() * 1e-4;
    R_gnss_pos_ = Eigen::Matrix3d::Identity() * 1.0;
    R_gnss_vel_ = Eigen::Matrix3d::Identity() * 0.1;

    // 初始化状态协方差
    P_ = StateCovariance::Identity() * 0.1;
}

bool GNSSINS::Initialize(const GNSSData& init_gnss) {
   
    state_.position = init_gnss.lla;
    state_.velocity = init_gnss.velocity;
    state_.attitude = Eigen::Matrix3d::Identity(); // 初始假设水平
    state_.bias_gyro = Eigen::Vector3d::Zero();
    state_.bias_accel = Eigen::Vector3d::Zero();

    x_.setZero();
    initialized_ = true;
    last_update_time_ = init_gnss.timestamp;

    std::cout << "GNSS/INS系统初始化完成" << std::endl;
    return true;
}

void GNSSINS::UpdateIMU(const IMUData& imu) {
   
    if (!initialized_) return;

    if (last_update_time_ > 0) {
   
        double dt = imu.timestamp - last_update_time_;
        if (dt > 0) {
   
            Predict(imu);
        }
    }

    last_imu_ = imu;
    last_update_time_ = imu.timestamp;
}

void GNSSINS::UpdateGNSS(const GNSSData& gnss) {
   
    if (!initialized_) {
   
        Initialize(gnss);
        return;
    }

    Correct(gnss);
}

void GNSSINS::Predict(const IMUData& imu) {
   
    double dt = imu.timestamp - last_update_time_;

    // 补偿零偏后的角速度和加速度
    Eigen::Vector3d gyro = imu.gyro - state_.bias_gyro;
    Eigen::Vector3d accel = imu.accel - state_.bias_accel;

    // 更新姿态
    UpdateAttitude(gyro, dt);

    // 转换加速度到导航系并去除重力
    Eigen::Vector3d acc_n = state_.attitude * accel;
    acc_n(2) += 9.7803267714; // 添加重力

    // 更新速度
    state_.velocity += acc_n * dt;

    // 更新位置 (简化处理)
    Eigen::Vector3d pos_lla = state_.position;
    double Rn = earth_a / sqrt(1 - earth_f * (2 - earth_f) * pow(sin(pos_lla(0)), 2));
    double Rm = Rn * (1 - earth_f * (2 - earth_f)) / (1 - earth_f * (2 - earth_f) * pow(sin(pos_lla(0)), 2));

    state_.position(0) += state_.velocity(0) * dt / (Rm + pos_lla(2)); // 纬度
    state_.position(1) += state_.velocity(1) * dt / ((Rn + pos_lla(2)) * cos(pos_lla(0))); // 经度
    state_.position(2) += -state_.velocity(2) * dt; // 高度

    // 预测误差协方差 (简化扩展卡尔曼滤波)
    // 这里需要计算状态转移矩阵F和过程噪声矩阵Q
    // 实际实现中需要完整的误差状态方程
}

void GNSSINS::Correct(const GNSSData& gnss) {
   
    // 量测向量
    Eigen::Matrix<double, 6, 1> z;
    z << gnss.lla - state_.position, gnss.velocity - state_.velocity;

    // 量测矩阵
    Eigen::Matrix<double, 6, STATE_DIM> H;
    H.setZero();
    H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // 位置
    H.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity(); // 速度

    // 量测噪声协方差
    Eigen::Matrix<double, 6, 6> R;
    R.setZero();
    R.block<3, 3>(0, 0) = R_gnss_pos_;
    R.block<3, 3>(3, 3) = R_gnss_vel_;

    // 卡尔曼增益
    Eigen::Matrix<double, STATE_DIM, 6> K = P_ * H.transpose() * 
                                          (H * P_ * H.transpose() + R).inverse();

    // 状态修正
    x_ = x_ + K * (z - H * x_);

    // 协方差更新
    P_ = (StateCovariance::Identity() - K * H) * P_;

    // 反馈到导航状态
    state_.position += x_.segment<3>(0);
    state_.velocity += x_.segment<3>(3);

    // 姿态修正
    Eigen::Vector3d phi = x_.segment<3>(6);
    Eigen::Matrix3d I_phi = Eigen::Matrix3d::Identity() - SkewSymmetric(phi);
    state_.attitude = state_.attitude * I_phi;

    // 零偏修正
    state_.bias_gyro += x_.segment<3>(9);
    state_.bias_accel += x_.segment<3>(12);

    // 重置误差状态
    x_.setZero();
}

void GNSSINS::UpdateAttitude(const Eigen::Vector3d& gyro, double dt) {
   
    // 使用四阶龙格库塔法更新姿态
    Eigen::Vector3d omega = gyro;

    // 计算姿态矩阵的导数
    Eigen::Matrix3d omega_skew = SkewSymmetric(omega);
    Eigen::Matrix3d dC = state_.attitude * omega_skew;

    // 更新姿态矩阵
    state_.attitude = state_.attitude + dC * dt;

    // 正交化姿态矩阵
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(state_.attitude, 
                                         Eigen::ComputeFullU | Eigen::ComputeFullV);
    state_.attitude = svd.matrixU() * svd.matrixV().transpose();
}

Eigen::Matrix3d GNSSINS::SkewSymmetric(const Eigen::Vector3d& v) {
   
    Eigen::Matrix3d M;
    M << 0, -v(2), v(1),
         v(2), 0, -v(0),
         -v(1), v(0), 0;
    return M;
}

GNSSINS::NavigationState GNSSINS::GetState() const {
   
    return state_;
}

Eigen::Vector3d GNSSINS::LLAToECEF(const Eigen::Vector3d& lla) {
   
    double lat = lla(0), lon = lla(1), alt = lla(2);
    double e2 = earth_f * (2 - earth_f);
    double N = earth_a / sqrt(1 - e2 * sin(lat) * sin(lat));

    Eigen::Vector3d ecef;
    ecef(0) = (N + alt) * cos(lat) * cos(lon);
    ecef(1) = (N + alt) * cos(lat) * sin(lon);
    ecef(2) = (N * (1 - e2) + alt) * sin(lat);

    return ecef;
}

Eigen::Vector3d GNSSINS::ECEFToLLA(const Eigen::Vector3d& ecef) {
   
    // 简化实现，实际需要迭代计算
    double x = ecef(0), y = ecef(1), z = ecef(2);
    double p = sqrt(x*x + y*y);
    double e2 = earth_f * (2 - earth_f);

    double lat = atan2(z, p * (1 - e2));
    double lon = atan2(y, x);
    double N = earth_a / sqrt(1 - e2 * sin(lat) * sin(lat));
    double alt = p / cos(lat) - N;

    return Eigen::Vector3d(lat, lon, alt);
}

使用示例 (main.cpp)

#include "GNSS_INS.h"
#include <random>
#include <fstream>

int main() {
   
    GNSSINS navigator;

    // 模拟数据生成
    std::default_random_engine generator;
    std::normal_distribution<double> noise(0.0, 0.1);

    // 初始化GNSS数据
    GNSSINS::GNSSData init_gnss;
    init_gnss.timestamp = 0.0;
    init_gnss.lla = Eigen::Vector3d(0.7, 1.3, 100.0); // 经纬高 (rad)
    init_gnss.velocity = Eigen::Vector3d(10.0, 0.0, 0.0); // NED速度

    // 初始化导航系统
    navigator.Initialize(init_gnss);

    // 模拟数据循环
    double dt = 0.01; // 100Hz IMU, 1Hz GNSS
    for (double t = dt; t < 10.0; t += dt) {
   
        // 生成IMU数据
        GNSSINS::IMUData imu;
        imu.timestamp = t;
        imu.gyro = Eigen::Vector3d(0.01, 0.02, 0.03) + 
                   Eigen::Vector3d(noise(generator), noise(generator), noise(generator)) * 0.001;
        imu.accel = Eigen::Vector3d(0.1, 0.0, 9.8) + 
                    Eigen::Vector3d(noise(generator), noise(generator), noise(generator)) * 0.01;

        navigator.UpdateIMU(imu);

        // 每秒更新一次GNSS
        if (fmod(t, 1.0) < dt) {
   
            GNSSINS::GNSSData gnss;
            gnss.timestamp = t;
            gnss.lla = init_gnss.lla + Eigen::Vector3d(t * 0.0001, t * 0.0002, -t * 0.1) +
                       Eigen::Vector3d(noise(generator), noise(generator), noise(generator)) * 0.1;
            gnss.velocity = Eigen::Vector3d(10.0 + sin(t)*0.1, cos(t)*0.1, 0.0) +
                           Eigen::Vector3d(noise(generator), noise(generator), noise(generator)) * 0.01;

            navigator.UpdateGNSS(gnss);

            auto state = navigator.GetState();
            std::cout << "Time: " << t 
                      << " Lat: " << state.position(0) 
                      << " Lon: " << state.position(1)
                      << " Alt: " << state.position(2) << std::endl;
        }
    }

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
project(GNSS_INS)

set(CMAKE_CXX_STANDARD 14)

find_package(Eigen3 REQUIRED)

add_executable(gnss_ins 
    main.cpp 
    GNSS_INS.cpp
)

target_link_libraries(gnss_ins Eigen3::Eigen)

参考代码 C++编写的关于GPS捷联惯性组合导航的程序 www.3dddown.com/ala/69797.html

关键特性

1. 松组合架构：GNSS和INS独立解算，通过卡尔曼滤波融合
2. 误差状态卡尔曼滤波：使用15状态误差模型
3. 实时处理：支持高频IMU和低频GNSS数据融合
4. 模块化设计：易于扩展和修改

扩展建议

• 添加紧组合模式，直接处理GNSS原始观测值
• 实现更精确的地球模型和机械编排算法
• 添加自适应卡尔曼滤波
• 集成RTK-GNSS支持
• 添加传感器标定和温度补偿