diff --git a/3rdparty/Fusion b/3rdparty/Fusion
new file mode 160000
index 00000000..53c67a79
--- /dev/null
+++ b/3rdparty/Fusion
@@ -0,0 +1 @@
+Subproject commit 53c67a79e0d08b82ffa975af28631dbb2a68529c
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e7a5962d..755e5665 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -435,6 +435,7 @@ endif()
 
 
 add_subdirectory(${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
+add_subdirectory(${THIRDPARTY_DIRECTORY}/Fusion/Fusion)
 
 add_subdirectory(core)
 add_subdirectory(core_hd_mapping)
diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt
index 68ac48c0..744c7617 100644
--- a/apps/lidar_odometry_step_1/CMakeLists.txt
+++ b/apps/lidar_odometry_step_1/CMakeLists.txt
@@ -42,11 +42,12 @@ target_include_directories(
           ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master
           ${LASZIP_INCLUDE_DIR}/LASzip/include
           ${THIRDPARTY_DIRECTORY}/observation_equations/codes
-          ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
+          ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp
+          ${THIRDPARTY_DIRECTORY}/Fusion/Fusion)
 
 target_link_libraries(
           lidar_odometry_step_1
-          PRIVATE vqf
+          PRIVATE vqf Fusion
           unordered_dense::unordered_dense
           spdlog::spdlog
           UTL::include
@@ -99,11 +100,12 @@ target_include_directories(
           ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master
           ${LASZIP_INCLUDE_DIR}/LASzip/include
           ${THIRDPARTY_DIRECTORY}/observation_equations/codes
-          ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp)
+          ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp
+          ${THIRDPARTY_DIRECTORY}/Fusion/Fusion)
 
 target_link_libraries(
           drag_folder_with_mandeye_data_and_drop_here-precision_forestry
-          PRIVATE vqf
+          PRIVATE vqf Fusion
           unordered_dense::unordered_dense
           spdlog::spdlog
           UTL::include
diff --git a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp
index 93431d7b..690bf0b8 100644
--- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp
+++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp
@@ -717,37 +717,48 @@ void settings_gui()
 
             ImGui::NewLine();
 
-            static int fusionConvention; // 0=NWU, 1=ENU, 2=NED
-            // initialize if none selected
-            if (fusionConvention < 0 || fusionConvention > 2)
-                fusionConvention = 0;
-
-            ImGui::Text("AHRS convention: ");
+            ImGui::Checkbox("Use VQF (instead of Fusion/Madgwick)", &params.use_vqf);
             if (ImGui::IsItemHovered())
-                ImGui::SetTooltip(
-                    "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame");
+                ImGui::SetTooltip("Unchecked = Fusion (Madgwick complementary filter, default)\nChecked = VQF (Versatile Quaternion-based Filter)");
 
-            ImGui::SameLine();
-            ImGui::RadioButton("NWU", &fusionConvention, 0);
-            if (ImGui::IsItemHovered())
-                ImGui::SetTooltip("North West Up");
-            ImGui::SameLine();
-            ImGui::RadioButton("ENU", &fusionConvention, 1);
-            if (ImGui::IsItemHovered())
-                ImGui::SetTooltip("East North Up");
-            ImGui::SameLine();
-            ImGui::RadioButton("NED", &fusionConvention, 2);
-            if (ImGui::IsItemHovered())
-                ImGui::SetTooltip("North East Down");
+            if (!params.use_vqf)
+            {
+                static int fusionConvention; // 0=NWU, 1=ENU, 2=NED
+                if (fusionConvention < 0 || fusionConvention > 2)
+                    fusionConvention = 0;
 
-            // then update your bools if you still need them
-            params.fusionConventionNwu = (fusionConvention == 0);
-            params.fusionConventionEnu = (fusionConvention == 1);
-            params.fusionConventionNed = (fusionConvention == 2);
+                ImGui::Text("Fusion convention: ");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip(
+                        "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame");
+
+                ImGui::SameLine();
+                ImGui::RadioButton("NWU", &fusionConvention, 0);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("North West Up");
+                ImGui::SameLine();
+                ImGui::RadioButton("ENU", &fusionConvention, 1);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("East North Up");
+                ImGui::SameLine();
+                ImGui::RadioButton("NED", &fusionConvention, 2);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("North East Down");
+
+                params.fusionConventionNwu = (fusionConvention == 0);
+                params.fusionConventionEnu = (fusionConvention == 1);
+                params.fusionConventionNed = (fusionConvention == 2);
+
+                ImGui::InputDouble("Fusion gain", &params.fusion_gain, 0.0, 0.0, "%.3f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Complementary filter gain (0-1). Higher = more accelerometer trust.");
+            }
 
             ImGui::NewLine();
 
             ImGui::Checkbox("Use motion from previous step", &params.use_motion_from_previous_step);
+            if (params.use_vqf)
+            {
             ImGui::InputDouble("VQF tauAcc [s]", &params.vqf_tauAcc, 0.0, 0.0, "%.3f");
             if (ImGui::IsItemHovered())
             {
@@ -901,6 +912,7 @@ void settings_gui()
                         "rejection, correction uses\nthis factor to increase the time constant. Default: 2.0");
                 ImGui::TreePop();
             }
+            } // end if (params.use_vqf)
 
             ImGui::PopItemWidth();
         }
diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp
index e161d74c..f68216d5 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry.cpp
+++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp
@@ -314,67 +314,150 @@ bool load_data(
     }
 }
 
-void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, const LidarOdometryParams& params, bool debugMsg)
+void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryParams& params, bool debugMsg)
 {
     UTL_PROFILER_SCOPE("calculate_trajectory");
 
+    const float RAD_TO_DEG = 180.0f / static_cast<float>(M_PI);
     int counter = 1;
-    double previous_time_stamp = 0.0;
 
     std::cout << "start calculating trajectory.." << std::endl;
 
-    // Compute average dt for VQF initialization
-    double avg_dt = 1.0 / 200.0;
-    if (imu_data.size() >= 2)
+    if (params.use_vqf)
     {
-        double t0 = std::get<0>(imu_data.front()).first;
-        double t1 = std::get<0>(imu_data.back()).first;
-        if (t1 > t0)
-            avg_dt = (t1 - t0) / static_cast<double>(imu_data.size() - 1);
-    }
+        // ---- VQF path ----
+        double avg_dt = 1.0 / 200.0;
+        if (imu_data.size() >= 2)
+        {
+            double t0 = std::get<0>(imu_data.front()).first;
+            double t1 = std::get<0>(imu_data.back()).first;
+            if (t1 > t0)
+                avg_dt = (t1 - t0) / static_cast<double>(imu_data.size() - 1);
+        }
 
-    VQFParams vqf_params = buildVQFParams(params);
-    VQF vqf(vqf_params, avg_dt);
-    std::cout << "AHRS: VQF (tauAcc=" << vqf_params.tauAcc << ", useMag=" << (params.vqf_useMagnetometer ? "true" : "false")
-              << ", dt=" << avg_dt << ", samples=" << imu_data.size() << ")" << std::endl;
+        VQFParams vqf_params = buildVQFParams(params);
+        VQF vqf(vqf_params, avg_dt);
+        std::cout << "AHRS: VQF (tauAcc=" << vqf_params.tauAcc << ", useMag=" << (params.vqf_useMagnetometer ? "true" : "false")
+                  << ", dt=" << avg_dt << ", samples=" << imu_data.size() << ")" << std::endl;
 
-    for (const auto& [timestamp_pair, gyr, acc] : imu_data)
-    {
-        // IMU data: gyro in rad/s, acc in g
-        // VQF expects: gyro in rad/s, acc in m/s²
-        const double g = 9.81;
-        vqf_real_t gyr_vqf[3] = { static_cast<double>(gyr.x()), static_cast<double>(gyr.y()), static_cast<double>(gyr.z()) };
-        vqf_real_t acc_vqf[3] = { static_cast<double>(acc.x()) * g, static_cast<double>(acc.y()) * g, static_cast<double>(acc.z()) * g };
+        for (const auto& [timestamp_pair, gyr, acc] : imu_data)
+        {
+            const double g = 9.81;
+            vqf_real_t gyr_vqf[3] = { static_cast<double>(gyr.x()), static_cast<double>(gyr.y()), static_cast<double>(gyr.z()) };
+            vqf_real_t acc_vqf[3] = { static_cast<double>(acc.x()) * g, static_cast<double>(acc.y()) * g, static_cast<double>(acc.z()) * g };
 
-        vqf.update(gyr_vqf, acc_vqf);
+            vqf.update(gyr_vqf, acc_vqf);
 
-        vqf_real_t quat[4];
-        if (params.vqf_useMagnetometer)
-            vqf.getQuat9D(quat);
-        else
-            vqf.getQuat6D(quat);
-        Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]);
-        Eigen::Affine3d t{ Eigen::Matrix4d::Identity() };
-        t.rotate(d);
-
-        // Store gyro/acc in original units for RawIMUData
-        RawIMUData rawImuData{ timestamp_pair.first,
-                               { static_cast<double>(acc.x()), static_cast<double>(acc.y()), static_cast<double>(acc.z()) },
-                               { static_cast<double>(gyr.x()), static_cast<double>(gyr.y()), static_cast<double>(gyr.z()) } };
-        trajectory[timestamp_pair.first] = std::make_tuple(t.matrix(), timestamp_pair.second, rawImuData);
-
-        if (debugMsg)
+            vqf_real_t quat[4];
+            if (params.vqf_useMagnetometer)
+                vqf.getQuat9D(quat);
+            else
+                vqf.getQuat6D(quat);
+            Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]);
+            Eigen::Affine3d t{ Eigen::Matrix4d::Identity() };
+            t.rotate(d);
+
+            RawIMUData rawImuData{ timestamp_pair.first,
+                                   { static_cast<double>(acc.x()), static_cast<double>(acc.y()), static_cast<double>(acc.z()) },
+                                   { static_cast<double>(gyr.x()), static_cast<double>(gyr.y()), static_cast<double>(gyr.z()) } };
+            trajectory[timestamp_pair.first] = std::make_tuple(t.matrix(), timestamp_pair.second, rawImuData);
+
+            if (debugMsg)
+            {
+                counter++;
+                if (counter % 100 == 0)
+                {
+                    Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI);
+                    std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of "
+                              << imu_data.size() << "]" << std::endl;
+                }
+            }
+        }
+    }
+    else
+    {
+        // ---- Fusion (Madgwick) path ----
+        FusionAhrs ahrs;
+        FusionAhrsInitialise(&ahrs);
+
+        if (params.fusionConventionNwu)
+            ahrs.settings.convention = FusionConventionNwu;
+        else if (params.fusionConventionEnu)
+            ahrs.settings.convention = FusionConventionEnu;
+        else if (params.fusionConventionNed)
+            ahrs.settings.convention = FusionConventionNed;
+        ahrs.settings.gain = static_cast<float>(params.fusion_gain);
+
+        // Gyro bias estimation from first 1000 stationary samples
+        double bias_gyr_x = 0.0, bias_gyr_y = 0.0, bias_gyr_z = 0.0;
+        if (params.use_removie_imu_bias_from_first_stationary_scan && imu_data.size() > 1000)
         {
-            counter++;
-            if (counter % 100 == 0)
+            std::vector<double> gyr_x, gyr_y, gyr_z;
+            gyr_x.reserve(1000); gyr_y.reserve(1000); gyr_z.reserve(1000);
+            for (int i = 0; i < 1000; i++)
             {
-                Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI);
-                std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of "
-                          << imu_data.size() << "]" << std::endl;
+                const auto& [timestamp_pair, gyr, acc] = imu_data[i];
+                gyr_x.push_back(gyr.x() * RAD_TO_DEG);
+                gyr_y.push_back(gyr.y() * RAD_TO_DEG);
+                gyr_z.push_back(gyr.z() * RAD_TO_DEG);
             }
+            std::nth_element(gyr_x.begin(), gyr_x.begin() + 500, gyr_x.end());
+            std::nth_element(gyr_y.begin(), gyr_y.begin() + 500, gyr_y.end());
+            std::nth_element(gyr_z.begin(), gyr_z.begin() + 500, gyr_z.end());
+            bias_gyr_x = gyr_x[500]; bias_gyr_y = gyr_y[500]; bias_gyr_z = gyr_z[500];
+            params.estimated_gyro_bias_dps = Eigen::Vector3d(bias_gyr_x, bias_gyr_y, bias_gyr_z);
+
+            std::cout << "------------------\nGYRO BIAS\n"
+                      << "bias_gyr_x [deg/s]: " << bias_gyr_x << "\n"
+                      << "bias_gyr_y [deg/s]: " << bias_gyr_y << "\n"
+                      << "bias_gyr_z [deg/s]: " << bias_gyr_z << "\n------------------" << std::endl;
         }
 
-        previous_time_stamp = timestamp_pair.first;
+        std::cout << "AHRS: Fusion (gain=" << params.fusion_gain << ", samples=" << imu_data.size() << ")" << std::endl;
+
+        bool first = true;
+        double last_ts = 0.0;
+
+        for (const auto& [timestamp_pair, gyr, acc] : imu_data)
+        {
+            const FusionVector gyroscope = { static_cast<float>(gyr.x() * RAD_TO_DEG) - static_cast<float>(bias_gyr_x),
+                                             static_cast<float>(gyr.y() * RAD_TO_DEG) - static_cast<float>(bias_gyr_y),
+                                             static_cast<float>(gyr.z() * RAD_TO_DEG) - static_cast<float>(bias_gyr_z) };
+            const FusionVector accelerometer = { acc.x(), acc.y(), acc.z() };
+
+            if (first)
+            {
+                FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0f / 200.0f);
+                first = false;
+            }
+            else
+            {
+                float ts_diff = static_cast<float>(timestamp_pair.first - last_ts);
+                FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff);
+            }
+            last_ts = timestamp_pair.first;
+
+            FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs);
+            Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z };
+            Eigen::Affine3d t{ Eigen::Matrix4d::Identity() };
+            t.rotate(d);
+
+            RawIMUData rawImuData{ timestamp_pair.first,
+                                   { static_cast<double>(acc.x()), static_cast<double>(acc.y()), static_cast<double>(acc.z()) },
+                                   { static_cast<double>(gyr.x()), static_cast<double>(gyr.y()), static_cast<double>(gyr.z()) } };
+            trajectory[timestamp_pair.first] = std::make_tuple(t.matrix(), timestamp_pair.second, rawImuData);
+
+            if (debugMsg)
+            {
+                counter++;
+                if (counter % 100 == 0)
+                {
+                    const FusionEuler euler = FusionQuaternionToEuler(quat);
+                    std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw
+                              << " [" << counter << " of " << imu_data.size() << "]" << std::endl;
+                }
+            }
+        }
     }
 }
 
diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h
index 9ae631ff..4035ab6f 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry.h
+++ b/apps/lidar_odometry_step_1/lidar_odometry.h
@@ -23,7 +23,7 @@ bool load_data(
     std::vector<std::vector<Point3Di>>& pointsPerFile,
     Imu& imu_data,
     bool debugMsg);
-void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, const LidarOdometryParams& params, bool debugMsg);
+void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryParams& params, bool debugMsg);
 bool compute_step_1(
     const std::vector<std::vector<Point3Di>>& pointsPerFile,
     LidarOdometryParams& params,
diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
index e5d3c35b..9a8868ce 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
+++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
@@ -474,52 +474,95 @@ void alternative_approach()
                     std::cout << fn << std::endl;
                     imu_data.insert(std::end(imu_data), std::begin(imu), std::end(imu)); });
 
-            // Compute average dt for VQF initialization
-            double avg_dt = 1.0 / 200.0;
-            if (imu_data.size() >= 2)
+            std::map<double, Eigen::Matrix4d> trajectory;
+            int counter = 1;
+            const float RAD_TO_DEG = 180.0f / static_cast<float>(M_PI);
+
+            if (params.use_vqf)
             {
-                double t0 = std::get<0>(imu_data.front()).first;
-                double t1 = std::get<0>(imu_data.back()).first;
-                if (t1 > t0)
-                    avg_dt = (t1 - t0) / static_cast<double>(imu_data.size() - 1);
-            }
+                double avg_dt = 1.0 / 200.0;
+                if (imu_data.size() >= 2)
+                {
+                    double t0 = std::get<0>(imu_data.front()).first;
+                    double t1 = std::get<0>(imu_data.back()).first;
+                    if (t1 > t0)
+                        avg_dt = (t1 - t0) / static_cast<double>(imu_data.size() - 1);
+                }
 
-            VQFParams vqf_params = buildVQFParams(params);
-            VQF vqf(vqf_params, avg_dt);
+                VQFParams vqf_params = buildVQFParams(params);
+                VQF vqf(vqf_params, avg_dt);
 
-            std::map<double, Eigen::Matrix4d> trajectory;
+                for (const auto& [timestamp_pair, gyr, acc] : imu_data)
+                {
+                    const double g = 9.81;
+                    vqf_real_t gyr_vqf[3] = { static_cast<double>(gyr.x()), static_cast<double>(gyr.y()), static_cast<double>(gyr.z()) };
+                    vqf_real_t acc_vqf[3] = { static_cast<double>(acc.x()) * g, static_cast<double>(acc.y()) * g, static_cast<double>(acc.z()) * g };
+
+                    vqf.update(gyr_vqf, acc_vqf);
+
+                    vqf_real_t quat[4];
+                    if (params.vqf_useMagnetometer)
+                        vqf.getQuat9D(quat);
+                    else
+                        vqf.getQuat6D(quat);
+                    Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]);
+                    Eigen::Affine3d t{ Eigen::Matrix4d::Identity() };
+                    t.rotate(d);
+                    trajectory[timestamp_pair.first] = t.matrix();
+                    counter++;
+                    if (counter % 100 == 0)
+                    {
+                        Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI);
+                        std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of " << imu_data.size() << "]" << std::endl;
+                    }
+                }
+            }
+            else
+            {
+                FusionAhrs ahrs;
+                FusionAhrsInitialise(&ahrs);
+                if (params.fusionConventionNwu) ahrs.settings.convention = FusionConventionNwu;
+                else if (params.fusionConventionEnu) ahrs.settings.convention = FusionConventionEnu;
+                else if (params.fusionConventionNed) ahrs.settings.convention = FusionConventionNed;
+                ahrs.settings.gain = params.fusion_gain;
 
-            int counter = 1;
+                bool first = true;
+                double last_ts = 0.0;
 
-            for (const auto& [timestamp_pair, gyr, acc] : imu_data)
-            {
-                const double g = 9.81;
-                vqf_real_t gyr_vqf[3] = {
-                    static_cast<double>(gyr.x()),
-                    static_cast<double>(gyr.y()),
-                    static_cast<double>(gyr.z()) };
-                vqf_real_t acc_vqf[3] = {
-                    static_cast<double>(acc.x()) * g,
-                    static_cast<double>(acc.y()) * g,
-                    static_cast<double>(acc.z()) * g };
-
-                vqf.update(gyr_vqf, acc_vqf);
-
-                vqf_real_t quat[4];
-                if (params.vqf_useMagnetometer)
-                    vqf.getQuat9D(quat);
-                else
-                    vqf.getQuat6D(quat);
-                Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]);
-                Eigen::Affine3d t{ Eigen::Matrix4d::Identity() };
-                t.rotate(d);
-
-                trajectory[timestamp_pair.first] = t.matrix();
-                counter++;
-                if (counter % 100 == 0)
+                for (const auto& [timestamp_pair, gyr, acc] : imu_data)
                 {
-                    Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI);
-                    std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of " << imu_data.size() << "]" << std::endl;
+                    const FusionVector gyroscope = { static_cast<float>(gyr.x() * RAD_TO_DEG),
+                                                     static_cast<float>(gyr.y() * RAD_TO_DEG),
+                                                     static_cast<float>(gyr.z() * RAD_TO_DEG) };
+                    const FusionVector accelerometer = { acc.x(), acc.y(), acc.z() };
+
+                    if (first)
+                    {
+                        FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0f / 200.0f);
+                        first = false;
+                    }
+                    else
+                    {
+                        float ts_diff = static_cast<float>(timestamp_pair.first - last_ts);
+                        if (ts_diff < 0.01f)
+                            FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff);
+                        else
+                            FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0f / 200.0f);
+                    }
+                    last_ts = timestamp_pair.first;
+
+                    FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs);
+                    Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z };
+                    Eigen::Affine3d t{ Eigen::Matrix4d::Identity() };
+                    t.rotate(d);
+                    trajectory[timestamp_pair.first] = t.matrix();
+                    counter++;
+                    if (counter % 100 == 0)
+                    {
+                        const FusionEuler euler = FusionQuaternionToEuler(quat);
+                        std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw
+                                  << " [" << counter << " of " << imu_data.size() << "]" << std::endl;
+                    }
                 }
             }
 
@@ -883,33 +926,44 @@ void settings_gui()
 
             ImGui::NewLine();
 
-            static int fusionConvention; // 0=NWU, 1=ENU, 2=NED
-            // initialize if none selected
-            if (fusionConvention < 0 || fusionConvention > 2)
-                fusionConvention = 0;
-
-            ImGui::Text("AHRS convention: ");
+            // AHRS type selection
+            ImGui::Checkbox("Use VQF (instead of Fusion/Madgwick)", &params.use_vqf);
             if (ImGui::IsItemHovered())
-                ImGui::SetTooltip(
-                    "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame");
+                ImGui::SetTooltip("Unchecked = Fusion (Madgwick complementary filter, default)\nChecked = VQF (Versatile Quaternion-based Filter)");
 
-            ImGui::SameLine();
-            ImGui::RadioButton("NWU", &fusionConvention, 0);
-            if (ImGui::IsItemHovered())
-                ImGui::SetTooltip("North West Up");
-            ImGui::SameLine();
-            ImGui::RadioButton("ENU", &fusionConvention, 1);
-            if (ImGui::IsItemHovered())
-                ImGui::SetTooltip("East North Up");
-            ImGui::SameLine();
-            ImGui::RadioButton("NED", &fusionConvention, 2);
-            if (ImGui::IsItemHovered())
-                ImGui::SetTooltip("North East Down");
+            if (!params.use_vqf)
+            {
+                // Fusion-specific parameters
+                static int fusionConvention; // 0=NWU, 1=ENU, 2=NED
+                if (fusionConvention < 0 || fusionConvention > 2)
+                    fusionConvention = 0;
 
-            // then update your bools if you still need them
-            params.fusionConventionNwu = (fusionConvention == 0);
-            params.fusionConventionEnu = (fusionConvention == 1);
-            params.fusionConventionNed = (fusionConvention == 2);
+                ImGui::Text("Fusion convention: ");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip(
+                        "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame");
+
+                ImGui::SameLine();
+                ImGui::RadioButton("NWU", &fusionConvention, 0);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("North West Up");
+                ImGui::SameLine();
+                ImGui::RadioButton("ENU", &fusionConvention, 1);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("East North Up");
+                ImGui::SameLine();
+                ImGui::RadioButton("NED", &fusionConvention, 2);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("North East Down");
+
+                params.fusionConventionNwu = (fusionConvention == 0);
+                params.fusionConventionEnu = (fusionConvention == 1);
+                params.fusionConventionNed = (fusionConvention == 2);
+
+                ImGui::InputDouble("Fusion gain", &params.fusion_gain, 0.0, 0.0, "%.3f");
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Complementary filter gain (0-1). Higher = more accelerometer trust.");
+            }
 
             ImGui::NewLine();
 
@@ -923,11 +977,13 @@ void settings_gui()
                     "Euler, gravity comp., SM velocity",
                     "Trapezoidal, gravity comp., SM velocity",
                     "Kalman, gravity comp., SM velocity",
-                    "Euler, gravity comp., VQF velocity",
-                    "Trapezoidal, gravity comp., VQF velocity",
-                    "Kalman, gravity comp., VQF velocity" };
+                    "Euler, gravity comp., AHRS velocity",
+                    "Trapezoidal, gravity comp., AHRS velocity",
+                    "Kalman, gravity comp., AHRS velocity" };
                 ImGui::Combo("IMU preintegration method", &params.imu_preintegration_method, methods, IM_ARRAYSIZE(methods));
             }
+            if (params.use_vqf)
+            {
             ImGui::InputDouble("VQF tauAcc [s]", &params.vqf_tauAcc, 0.0, 0.0, "%.3f");
             if (ImGui::IsItemHovered())
             {
@@ -1070,6 +1126,7 @@ void settings_gui()
                         "rejection, correction uses\nthis factor to increase the time constant. Default: 2.0");
                 ImGui::TreePop();
             }
+            } // end if (params.use_vqf)
 
             ImGui::PopItemWidth();
         }
diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h
index e4985b51..79fc86b9 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h
+++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h
@@ -14,6 +14,7 @@
 #include <common/include/cauchy.h>
 #include <laszip/laszip_api.h>
 #include <nlohmann/json.hpp>
+#include <Fusion.h>
 #include <vqf.hpp>
 
 #include <Core/ndt.h>
@@ -65,10 +66,14 @@ struct LidarOdometryParams
     double threshould_output_filter = 0.5; // for export --> all points xyz.norm() < threshould_output_filter will be removed
     int min_counter_concatenated_trajectory_nodes = 10; // for export
 
-    // AHRS (VQF) — fusionConvention params kept for TOML backwards compatibility
+    // AHRS type selection: false = Fusion (Madgwick, default), true = VQF
+    bool use_vqf = false;
+
+    // Fusion (Madgwick) AHRS parameters
     bool fusionConventionNwu = true;
     bool fusionConventionEnu = false;
     bool fusionConventionNed = false;
+    double fusion_gain = 0.5; // complementary filter gain (0-1, higher = more accelerometer trust)
 
     // VQF core
     double vqf_tauAcc = 0.5; // accelerometer time constant [s] (higher = more gyro trust)
@@ -186,6 +191,7 @@ struct LidarOdometryParams
 #endif
 
     bool use_removie_imu_bias_from_first_stationary_scan = false;
+    Eigen::Vector3d estimated_gyro_bias_dps = Eigen::Vector3d::Zero(); // runtime: gyro bias in deg/s from stationary samples
 
     // IMU preintegration
     bool use_imu_preintegration = false;
diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
index f4caecf7..39576215 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
+++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
@@ -2416,6 +2416,15 @@ bool process_worker_step_1(
         if (params.use_imu_preintegration)
         {
             IntegrationParams imu_params;
+            imu_params.use_vqf = params.use_vqf;
+            imu_params.fusion_gain = params.fusion_gain;
+            if (params.fusionConventionEnu)
+                imu_params.fusion_convention = AhrsConvention::ENU;
+            else if (params.fusionConventionNed)
+                imu_params.fusion_convention = AhrsConvention::NED;
+            else
+                imu_params.fusion_convention = AhrsConvention::NWU;
+            imu_params.gyro_bias_dps = params.estimated_gyro_bias_dps;
             auto method = static_cast<PreintegrationMethod>(params.imu_preintegration_method);
 
             // Compute IMU time span for velocity estimation
@@ -2425,11 +2434,11 @@ bool process_worker_step_1(
 
             if (total_imu_time > 0.0)
             {
-                bool uses_vqf_velocity = (method == PreintegrationMethod::euler_gravity_vqf_vel ||
-                                          method == PreintegrationMethod::trapezoidal_gravity_vqf_vel ||
-                                          method == PreintegrationMethod::kalman_gravity_vqf_vel);
+                bool uses_ahrs_velocity = (method == PreintegrationMethod::euler_gravity_ahrs_vel ||
+                                           method == PreintegrationMethod::trapezoidal_gravity_ahrs_vel ||
+                                           method == PreintegrationMethod::kalman_gravity_ahrs_vel);
 
-                if (uses_vqf_velocity)
+                if (uses_ahrs_velocity)
                 {
                     // Methods 5-7: SM-independent velocity
                     // Direction from VQF AHRS, speed from motion model displacement
diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h
index 6d650dab..b4001e49 100644
--- a/apps/lidar_odometry_step_1/toml_io.h
+++ b/apps/lidar_odometry_step_1/toml_io.h
@@ -34,6 +34,8 @@ class TomlIO
         { "decimation", &LidarOdometryParams::decimation },
         { "threshould_output_filter", &LidarOdometryParams::threshould_output_filter },
         { "min_counter_concatenated_trajectory_nodes", &LidarOdometryParams::min_counter_concatenated_trajectory_nodes },
+        { "use_vqf", &LidarOdometryParams::use_vqf },
+        { "fusion_gain", &LidarOdometryParams::fusion_gain },
         { "fusionConventionNwu", &LidarOdometryParams::fusionConventionNwu },
         { "fusionConventionEnu", &LidarOdometryParams::fusionConventionEnu },
         { "fusionConventionNed", &LidarOdometryParams::fusionConventionNed },
@@ -129,7 +131,9 @@ class TomlIO
             "threshould_output_filter",
             "min_counter_concatenated_trajectory_nodes" } },
         { "ahrs_vqf",
-          { "fusionConventionNwu",
+          { "use_vqf",
+            "fusion_gain",
+            "fusionConventionNwu",
             "fusionConventionEnu",
             "fusionConventionNed",
             "vqf_tauAcc",
diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
index eb54f12b..46660c97 100644
--- a/core/CMakeLists.txt
+++ b/core/CMakeLists.txt
@@ -46,7 +46,7 @@ function(add_core_target target_name with_gui)
 
     add_library(${target_name} STATIC ${SOURCES})
     target_compile_definitions(${target_name} PRIVATE ${DEFINES})
-    target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog vqf)
+    target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog vqf Fusion)
     target_include_directories(${target_name} PRIVATE
         include
         ${EIGEN3_INCLUDE_DIR}
@@ -55,6 +55,7 @@ function(add_core_target target_name with_gui)
         ${THIRDPARTY_DIRECTORY}/observation_equations/codes
         ${EXTERNAL_LIBRARIES_DIRECTORY}/include
         ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp
+        ${THIRDPARTY_DIRECTORY}/Fusion/Fusion
     )
 
     target_link_libraries(${target_name} PRIVATE PROJ::proj)
diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h
index 876a1163..702e928b 100644
--- a/core/include/imu_preintegration.h
+++ b/core/include/imu_preintegration.h
@@ -6,6 +6,13 @@
 #include <vector>
 #include <vqf.hpp>
 
+enum class AhrsConvention
+{
+    NWU = 0,
+    ENU = 1,
+    NED = 2
+};
+
 struct IntegrationParams
 {
     bool accel_units_in_g = true;
@@ -14,6 +21,11 @@ struct IntegrationParams
     double max_dt_threshold = 0.1; // seconds
     Eigen::Vector3d initial_velocity = Eigen::Vector3d::Zero();
     double vqf_tauAcc = 0.5; // VQF accelerometer time constant [s]
+    // AHRS selection for per-worker orientation estimation (methods 5-7)
+    bool use_vqf = true; // true = VQF, false = Fusion (Madgwick)
+    double fusion_gain = 0.5; // Fusion complementary filter gain (0-1)
+    AhrsConvention fusion_convention = AhrsConvention::NWU;
+    Eigen::Vector3d gyro_bias_dps = Eigen::Vector3d::Zero(); // gyro bias in deg/s (for Fusion only)
 };
 
 namespace imu_utils
@@ -107,9 +119,9 @@ enum class PreintegrationMethod
     euler_gravity_sm_vel = 2,
     trapezoidal_gravity_sm_vel = 3,
     kalman_gravity_sm_vel = 4,
-    euler_gravity_vqf_vel = 5,
-    trapezoidal_gravity_vqf_vel = 6,
-    kalman_gravity_vqf_vel = 7,
+    euler_gravity_ahrs_vel = 5,
+    trapezoidal_gravity_ahrs_vel = 6,
+    kalman_gravity_ahrs_vel = 7,
 };
 
 inline const char* to_string(PreintegrationMethod method)
@@ -121,9 +133,9 @@ inline const char* to_string(PreintegrationMethod method)
     case PreintegrationMethod::euler_gravity_sm_vel: return "Euler, gravity comp., SM velocity";
     case PreintegrationMethod::trapezoidal_gravity_sm_vel: return "Trapezoidal, gravity comp., SM velocity";
     case PreintegrationMethod::kalman_gravity_sm_vel: return "Kalman, gravity comp., SM velocity";
-    case PreintegrationMethod::euler_gravity_vqf_vel: return "Euler, gravity comp., VQF velocity";
-    case PreintegrationMethod::trapezoidal_gravity_vqf_vel: return "Trapezoidal, gravity comp., VQF velocity";
-    case PreintegrationMethod::kalman_gravity_vqf_vel: return "Kalman, gravity comp., VQF velocity";
+    case PreintegrationMethod::euler_gravity_ahrs_vel: return "Euler, gravity comp., AHRS velocity";
+    case PreintegrationMethod::trapezoidal_gravity_ahrs_vel: return "Trapezoidal, gravity comp., AHRS velocity";
+    case PreintegrationMethod::kalman_gravity_ahrs_vel: return "Kalman, gravity comp., AHRS velocity";
     default: return "unknown";
     }
 }
diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp
index 5e200ee5..f825edbd 100644
--- a/core/src/imu_preintegration.cpp
+++ b/core/src/imu_preintegration.cpp
@@ -4,6 +4,7 @@
 #include <cmath>
 #include <iostream>
 
+#include <Fusion.h>
 #include <vqf.hpp>
 
 namespace imu_utils
@@ -49,9 +50,10 @@ std::vector<Eigen::Matrix3d> estimate_orientations(
 {
     std::vector<Eigen::Matrix3d> orientations;
     orientations.reserve(raw_imu_data.size());
+    orientations.push_back(initial_orientation);
 
-    // Compute average dt for VQF initialization
-    double avg_dt = 1.0 / 200.0; // default 200 Hz
+    // Compute average dt
+    double avg_dt = 1.0 / 200.0;
     if (raw_imu_data.size() >= 2)
     {
         double total_time = raw_imu_data.back().timestamp - raw_imu_data.front().timestamp;
@@ -59,9 +61,33 @@ std::vector<Eigen::Matrix3d> estimate_orientations(
             avg_dt = total_time / static_cast<double>(raw_imu_data.size() - 1);
     }
 
+    // Seed quaternion from initial_orientation
+    Eigen::Quaterniond init_q(initial_orientation);
+    init_q.normalize();
+
+    // Initialize selected AHRS
     VQF vqf(vqf_params, avg_dt);
+    FusionAhrs fusion_ahrs;
+    if (!params.use_vqf)
+    {
+        FusionAhrsInitialise(&fusion_ahrs);
+        switch (params.fusion_convention)
+        {
+        case AhrsConvention::NWU: fusion_ahrs.settings.convention = FusionConventionNwu; break;
+        case AhrsConvention::ENU: fusion_ahrs.settings.convention = FusionConventionEnu; break;
+        case AhrsConvention::NED: fusion_ahrs.settings.convention = FusionConventionNed; break;
+        }
+        fusion_ahrs.settings.gain = static_cast<float>(params.fusion_gain);
+        // Seed Fusion with initial orientation (Fusion has no internal bias estimation)
+        fusion_ahrs.quaternion = (FusionQuaternion){
+            .element = { .w = static_cast<float>(init_q.w()),
+                         .x = static_cast<float>(init_q.x()),
+                         .y = static_cast<float>(init_q.y()),
+                         .z = static_cast<float>(init_q.z()) } };
+        fusion_ahrs.initialising = false;
+    }
 
-    orientations.push_back(initial_orientation);
+    constexpr double RAD_TO_DEG = 180.0 / M_PI;
 
     for (size_t k = 1; k < raw_imu_data.size(); k++)
     {
@@ -72,24 +98,45 @@ std::vector<Eigen::Matrix3d> estimate_orientations(
             continue;
         }
 
-        // VQF expects: gyro in rad/s, acc in m/s²
-        // RawIMUData: gyro in rad/s, accel in g
-        const double g = 9.81;
-        vqf_real_t gyr[3] = {
-            raw_imu_data[k].guroscopes.x(),
-            raw_imu_data[k].guroscopes.y(),
-            raw_imu_data[k].guroscopes.z() };
-        vqf_real_t acc[3] = {
-            raw_imu_data[k].accelerometers.x() * g,
-            raw_imu_data[k].accelerometers.y() * g,
-            raw_imu_data[k].accelerometers.z() * g };
-
-        vqf.update(gyr, acc);
-
-        vqf_real_t quat[4]; // [w, x, y, z]
-        vqf.getQuat6D(quat);
-        Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
-        orientations.push_back(q.toRotationMatrix());
+        if (params.use_vqf)
+        {
+            // VQF expects: gyro in rad/s, acc in m/s²
+            const double g = 9.81;
+            vqf_real_t gyr[3] = {
+                raw_imu_data[k].guroscopes.x(),
+                raw_imu_data[k].guroscopes.y(),
+                raw_imu_data[k].guroscopes.z() };
+            vqf_real_t acc[3] = {
+                raw_imu_data[k].accelerometers.x() * g,
+                raw_imu_data[k].accelerometers.y() * g,
+                raw_imu_data[k].accelerometers.z() * g };
+
+            vqf.update(gyr, acc);
+
+            vqf_real_t quat[4];
+            vqf.getQuat6D(quat);
+            Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
+            orientations.push_back(q.toRotationMatrix());
+        }
+        else
+        {
+            // Fusion expects: gyro in deg/s, acc in g
+            // Subtract gyro bias (Fusion has no internal bias estimation, unlike VQF)
+            const FusionVector gyroscope = {
+                static_cast<float>(raw_imu_data[k].guroscopes.x() * RAD_TO_DEG - params.gyro_bias_dps.x()),
+                static_cast<float>(raw_imu_data[k].guroscopes.y() * RAD_TO_DEG - params.gyro_bias_dps.y()),
+                static_cast<float>(raw_imu_data[k].guroscopes.z() * RAD_TO_DEG - params.gyro_bias_dps.z()) };
+            const FusionVector accelerometer = {
+                static_cast<float>(raw_imu_data[k].accelerometers.x()),
+                static_cast<float>(raw_imu_data[k].accelerometers.y()),
+                static_cast<float>(raw_imu_data[k].accelerometers.z()) };
+
+            FusionAhrsUpdateNoMagnetometer(&fusion_ahrs, gyroscope, accelerometer, static_cast<float>(dt));
+
+            FusionQuaternion quat = FusionAhrsGetQuaternion(&fusion_ahrs);
+            Eigen::Quaterniond q(quat.element.w, quat.element.x, quat.element.y, quat.element.z);
+            orientations.push_back(q.toRotationMatrix());
+        }
     }
     return orientations;
 }
@@ -300,11 +347,11 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate(
         accel_model = std::make_unique<GravityCompensatedAcceleration>();
         integration_method = std::make_unique<KalmanFilterIntegration>();
         break;
-    case PreintegrationMethod::euler_gravity_vqf_vel:
-    case PreintegrationMethod::trapezoidal_gravity_vqf_vel:
-    case PreintegrationMethod::kalman_gravity_vqf_vel:
+    case PreintegrationMethod::euler_gravity_ahrs_vel:
+    case PreintegrationMethod::trapezoidal_gravity_ahrs_vel:
+    case PreintegrationMethod::kalman_gravity_ahrs_vel:
     {
-        // Per-worker VQF: estimate local orientations from IMU data
+        // Per-worker AHRS: estimate local orientations from IMU data (VQF or Fusion based on params.use_vqf)
         auto orientations = imu_utils::estimate_orientations(
             raw_imu_data, new_trajectory[0].rotation(), params, vqf_params);
 
@@ -313,9 +360,9 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate(
             imu_trajectory[k].linear() = orientations[k];
 
         accel_model = std::make_unique<GravityCompensatedAcceleration>();
-        if (method == PreintegrationMethod::euler_gravity_vqf_vel)
+        if (method == PreintegrationMethod::euler_gravity_ahrs_vel)
             integration_method = std::make_unique<EulerIntegration>();
-        else if (method == PreintegrationMethod::trapezoidal_gravity_vqf_vel)
+        else if (method == PreintegrationMethod::trapezoidal_gravity_ahrs_vel)
             integration_method = std::make_unique<TrapezoidalIntegration>();
         else
             integration_method = std::make_unique<KalmanFilterIntegration>();