main.cpp

#include <iostream>
#include <fstream>
#include <random>
#include <cmath>

// Simple 3D vector
struct Vec3 {
    double x;
    double y;
    double z;
};

// Vector addition
Vec3 add(Vec3 a, Vec3 b) {
    return {a.x + b.x, a.y + b.y, a.z + b.z};
}

// Vector multiplication by scalar
Vec3 mul(Vec3 a, double s) {
    return {a.x * s, a.y * s, a.z * s};
}

// True acceleration model
Vec3 true_acceleration(double t) {
    return {
        0.0,
        -0.8 * std::sin(0.5 * t),
        0.0
    };
}

int main() {
    const double T = 20.0;
    const double dt_ins = 0.01;
    const double dt_gnss = 0.1;

    const int steps = static_cast<int>(T / dt_ins);
    const int gnss_step = static_cast<int>(dt_gnss / dt_ins);

    std::ofstream file("trajectory.csv");

    file << "t,"
         << "true_x,true_y,true_z,"
         << "ins_x,ins_y,ins_z,"
         << "gnss_x,gnss_y,gnss_z\n";

    // Random number generator with fixed seed
    std::default_random_engine generator(42);

    // INS acceleration noise
    std::normal_distribution<double> acc_noise(0.0, 0.08);

    // GNSS position noise
    std::normal_distribution<double> gnss_noise(0.0, 0.5);

    // Constant INS bias in acceleration
    Vec3 acc_bias = {0.02, -0.015, 0.01};

    // Initial true state
    Vec3 true_pos = {0.0, 0.0, 100.0};
    Vec3 true_vel = {10.0, 3.0, 1.0};

    // Initial INS state
    Vec3 ins_pos = true_pos;
    Vec3 ins_vel = true_vel;

    for (int k = 0; k <= steps; ++k) {
        double t = k * dt_ins;

        Vec3 a_true = true_acceleration(t);

        // Simulated INS acceleration measurement
        Vec3 a_ins;
        a_ins.x = a_true.x + acc_bias.x + acc_noise(generator);
        a_ins.y = a_true.y + acc_bias.y + acc_noise(generator);
        a_ins.z = a_true.z + acc_bias.z + acc_noise(generator);

        // Integrate true motion
        true_vel = add(true_vel, mul(a_true, dt_ins));
        true_pos = add(true_pos, mul(true_vel, dt_ins));

        // Integrate INS motion
        ins_vel = add(ins_vel, mul(a_ins, dt_ins));
        ins_pos = add(ins_pos, mul(ins_vel, dt_ins));

        bool has_gnss = (k % gnss_step == 0);

        Vec3 gnss_pos;

        if (has_gnss) {
            gnss_pos.x = true_pos.x + gnss_noise(generator);
            gnss_pos.y = true_pos.y + gnss_noise(generator);
            gnss_pos.z = true_pos.z + gnss_noise(generator);
        } else {
            gnss_pos.x = NAN;
            gnss_pos.y = NAN;
            gnss_pos.z = NAN;
        }

        file << t << ","
             << true_pos.x << "," << true_pos.y << "," << true_pos.z << ","
             << ins_pos.x << "," << ins_pos.y << "," << ins_pos.z << ","
             << gnss_pos.x << "," << gnss_pos.y << "," << gnss_pos.z << "\n";
    }

    file.close();

    std::cout << "CSV file generated: trajectory.csv\n";

    return 0;
}