From af467a7ff889ee0a6755f8474f1795d975c03c7d Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Sun, 15 Dec 2024 15:08:51 +0100
Subject: [PATCH 01/15] Air density mechanism optimized.

Air density initialized according to ICAO std atmosphere.
Density re-calculation optimized for wave-flights.

Tested offline: OK.
---
 NAV_Algorithms/air_density_observer.cpp | 14 ++++++++++----
 NAV_Algorithms/air_density_observer.h   |  5 +++--
 NAV_Algorithms/atmosphere.h             | 11 ++++++++++-
 NAV_Algorithms/navigator.cpp            |  4 ++--
 NAV_Algorithms/navigator.h              |  4 +++-
 5 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp
index 7e4c1c5..ed058d6 100644
--- a/NAV_Algorithms/air_density_observer.cpp
+++ b/NAV_Algorithms/air_density_observer.cpp
@@ -42,7 +42,8 @@ air_data_result air_density_observer::feed_metering( float pressure, float MSL_a
   if( MSL_altitude < min_altitude)
     min_altitude = MSL_altitude;
 
-  if( false == altitude_trigger.process(MSL_altitude))
+  if( ((max_altitude - min_altitude) < 2 * MINIMUM_ALTITUDE_RANGE) &&
+      false == altitude_trigger.process(MSL_altitude))
     return air_data;
 
   if( ((max_altitude - min_altitude) < MINIMUM_ALTITUDE_RANGE) // ... forget this measurement
@@ -59,10 +60,15 @@ air_data_result air_density_observer::feed_metering( float pressure, float MSL_a
 //  Due to numeric effects, when using float the
 //  variance has been observed to be negative in some cases !
 //  Therefore using float this test can not be used.
- assert( result.variance_slope > 0);
- assert( result.variance_offset > 0);
+  if( ( result.variance_slope < 0) || ( result.variance_offset < 0))
+      {
+      density_QFF_calculator.reset();
+      air_data.valid=false;
+      return air_data;
+      }
 
- if( result.variance_slope < MAX_ALLOWED_VARIANCE)
+ if( (result.variance_slope < MAX_ALLOWED_SLOPE_VARIANCE) &&
+     (result.variance_offset < MAX_ALLOWED_OFFSET_VARIANCE))
     {
       air_data.QFF = (float)(result.y_offset);
       float density = 100.0f * (float)(result.slope) * -0.10194f; // div by -9.81f;
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index f5cbc0e..8a4581e 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -32,8 +32,9 @@
 typedef double evaluation_type;
 typedef uint64_t measurement_type;
 
-#define MAX_ALLOWED_VARIANCE	1e-9
-#define MINIMUM_ALTITUDE_RANGE	300.0f
+#define MAX_ALLOWED_SLOPE_VARIANCE 1e-9
+#define MAX_ALLOWED_OFFSET_VARIANCE 0.05
+#define MINIMUM_ALTITUDE_RANGE	250.0f
 #define ALTITUDE_TRIGGER_HYSTERESIS 50.0f
 
 //! this class maintains offset and slope of the air density measurement
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index 851ab12..13ab1eb 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -57,6 +57,15 @@ class atmosphere_t
   {
     density_correction_averager.settle(1.0f);
   }
+
+  //! set density to ICAO std density at present altitude
+  void normalize_density_correction( float GNSS_altitude)
+  {
+    float density_from_pressure = 1.0496346613e-5f * pressure + 0.1671546011f;
+    float ICAO_density_from_altitude = 1.2250 + GNSS_altitude * -1.1659e-4 + GNSS_altitude * GNSS_altitude * 3.786e-9;
+    density_correction = ICAO_density_from_altitude / density_from_pressure;
+    density_correction_averager.settle(density_correction);
+  }
   void update_density_correction( void)
   {
     density_correction_averager.respond(density_correction);
@@ -77,7 +86,7 @@ class atmosphere_t
   {
     return  (1.0496346613e-5f * pressure + 0.1671546011f) * density_correction_averager.get_output();
   }
-  float get_negative_altitude( void) const
+  float get_negative_pressure_altitude( void) const
   {
     float tmp = 8.104381531e-4f * pressure;
     return - tmp * tmp  + 0.20867299170f * pressure - 14421.43945f;
diff --git a/NAV_Algorithms/navigator.cpp b/NAV_Algorithms/navigator.cpp
index 5949775..30cb7ec 100644
--- a/NAV_Algorithms/navigator.cpp
+++ b/NAV_Algorithms/navigator.cpp
@@ -52,7 +52,7 @@ void navigator_t::update_at_100Hz (
       ahrs.get_nav_acceleration (),
       heading_vector,
       GNSS_negative_altitude,
-      atmosphere.get_negative_altitude(),
+      atmosphere.get_negative_pressure_altitude(),
       IAS,
       wind_observer.get_speed_compensator_wind(),
       (GNSS_fix_type != 0)
@@ -133,7 +133,7 @@ void navigator_t::report_data( output_data_t &d)
     d.slip_angle		= ahrs.getSlipAngle();
     d.pitch_angle		= ahrs.getPitchAngle();
     d.G_load			= ahrs.get_G_load();
-    d.pressure_altitude		= - atmosphere.get_negative_altitude();
+    d.pressure_altitude		= - atmosphere.get_negative_pressure_altitude();
     d.magnetic_disturbance	= ahrs.getMagneticDisturbance();
     d.air_density		= atmosphere.get_density();
     d.nav_induction_gnss 	= ahrs.get_nav_induction();
diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
index 1fe285f..f253521 100644
--- a/NAV_Algorithms/navigator.h
+++ b/NAV_Algorithms/navigator.h
@@ -119,7 +119,9 @@ class navigator_t
 
   void reset_altitude( void)
   {
-    flight_observer.reset( atmosphere.get_negative_altitude(), GNSS_negative_altitude);
+    // set density correction to the value that will let GNSS altitude match pressure altitude
+    atmosphere.normalize_density_correction( -GNSS_negative_altitude);
+    flight_observer.reset( atmosphere.get_negative_pressure_altitude(), GNSS_negative_altitude);
   }
   /**
    * @brief update pitot pressure

From ffc233fb09884678d44be330f23a5a723f4dc031 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Tue, 17 Dec 2024 20:16:50 +0100
Subject: [PATCH 02/15] Air density mechanism tuned.

Tested using SIL: OK.
---
 NAV_Algorithms/NAV_tuning_parameters.h  |  1 +
 NAV_Algorithms/air_density_observer.cpp | 27 ++++++++++++-------------
 NAV_Algorithms/air_density_observer.h   |  6 +++---
 NAV_Algorithms/atmosphere.h             |  5 +++--
 4 files changed, 20 insertions(+), 19 deletions(-)

diff --git a/NAV_Algorithms/NAV_tuning_parameters.h b/NAV_Algorithms/NAV_tuning_parameters.h
index 576dc91..0e76ae2 100644
--- a/NAV_Algorithms/NAV_tuning_parameters.h
+++ b/NAV_Algorithms/NAV_tuning_parameters.h
@@ -63,6 +63,7 @@
 #define INDUCTION_STD_DEVIATION_LIMIT	0.03 	//!< results outperforming this number will be used further on
 
 #define AIRBORNE_TRIGGER_SPEED		0.5f //!< speed-compensator vario value m/s
+#define AIR_DENSITY_LETHARGY_FACTOR 	0.85
 
 #define MAG_SCALE			10000.0f //!< scale factor for high-precision integer statistics
 #define FAST_SAMPLING_REQUENCY 		100.0f
diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp
index ed058d6..16b3acf 100644
--- a/NAV_Algorithms/air_density_observer.cpp
+++ b/NAV_Algorithms/air_density_observer.cpp
@@ -25,31 +25,31 @@
 #include "embedded_math.h"
 #include <air_density_observer.h>
 
-air_data_result air_density_observer::feed_metering( float pressure, float MSL_altitude)
+air_data_result air_density_observer::feed_metering( float pressure, float GNSS_altitude)
 {
   air_data_result air_data;
 
 #if DENSITY_MEASURMENT_COLLECTS_INTEGER
-  density_QFF_calculator.add_value( MSL_altitude * 100.0f, pressure);
+  density_QFF_calculator.add_value( GNSS_altitude * 100.0f, pressure);
 #else
   // use values normalized around 1.0
-  density_QFF_calculator.add_value( MSL_altitude * 1e-3, pressure * 1e-5);
+  density_QFF_calculator.add_value( GNSS_altitude * 1e-3, pressure * 1e-5);
 #endif
 
-  if( MSL_altitude > max_altitude)
-    max_altitude = MSL_altitude;
+  if( GNSS_altitude > max_altitude)
+    max_altitude = GNSS_altitude;
 
-  if( MSL_altitude < min_altitude)
-    min_altitude = MSL_altitude;
+  if( GNSS_altitude < min_altitude)
+    min_altitude = GNSS_altitude;
 
   if( ((max_altitude - min_altitude) < 2 * MINIMUM_ALTITUDE_RANGE) &&
-      false == altitude_trigger.process(MSL_altitude))
+      false == altitude_trigger.process(GNSS_altitude))
     return air_data;
 
   if( ((max_altitude - min_altitude) < MINIMUM_ALTITUDE_RANGE) // ... forget this measurement
     || (density_QFF_calculator.get_count() < 3000))
     {
-      max_altitude = min_altitude = MSL_altitude;
+      max_altitude = min_altitude = GNSS_altitude;
       density_QFF_calculator.reset();
       return air_data;
     }
@@ -74,17 +74,16 @@ air_data_result air_density_observer::feed_metering( float pressure, float MSL_a
       float density = 100.0f * (float)(result.slope) * -0.10194f; // div by -9.81f;
 
 #if DENSITY_MEASURMENT_COLLECTS_INTEGER
-      float pressure = density_QFF_calculator.get_mean_y();
+      float mean_altitude = density_QFF_calculator.get_mean_x() * 0.01f;
 #else
       float pressure = 1e5 * density_QFF_calculator.get_mean_y();
 #endif
-
-      float std_density = 1.0496346613e-5f * pressure + 0.1671546011f;
-      air_data.density_correction = density / std_density;
+      float ICAO_density_from_altitude = 1.2250 + mean_altitude * -1.1659e-4 + mean_altitude * mean_altitude * 3.786e-9;
+      air_data.density_correction = density / ICAO_density_from_altitude;
       air_data.valid = true;
     }
 
-  max_altitude = min_altitude = MSL_altitude;
+  max_altitude = min_altitude = GNSS_altitude;
   density_QFF_calculator.reset();
 
   return air_data;
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index 8a4581e..c4907a7 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -32,9 +32,9 @@
 typedef double evaluation_type;
 typedef uint64_t measurement_type;
 
-#define MAX_ALLOWED_SLOPE_VARIANCE 1e-9
-#define MAX_ALLOWED_OFFSET_VARIANCE 0.05
-#define MINIMUM_ALTITUDE_RANGE	250.0f
+#define MAX_ALLOWED_SLOPE_VARIANCE 1e-9f
+#define MAX_ALLOWED_OFFSET_VARIANCE 5.0f
+#define MINIMUM_ALTITUDE_RANGE	400.0f
 #define ALTITUDE_TRIGGER_HYSTERESIS 50.0f
 
 //! this class maintains offset and slope of the air density measurement
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index 13ab1eb..7e40970 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -28,6 +28,7 @@
 
 #include "embedded_math.h"
 #include <air_density_observer.h>
+#include <NAV_tuning_parameters.h>
 #include <pt2.h>
 
 #define RECIP_STD_DENSITY_TIMES_2 1.632f
@@ -120,8 +121,8 @@ class atmosphere_t
     air_data_result result = density_QFF_calculator.feed_metering( pressure, MSL_altitude);
       if( result.valid)
 	{
-	  QFF = result.QFF;
-	  density_correction = result.density_correction;
+	  QFF = QFF * AIR_DENSITY_LETHARGY_FACTOR + (1.0f - AIR_DENSITY_LETHARGY_FACTOR) * result.QFF;
+	  density_correction = density_correction * AIR_DENSITY_LETHARGY_FACTOR + (1.0f - AIR_DENSITY_LETHARGY_FACTOR) * result.density_correction;
 	}
     }
 

From 40ac44de07ff71891a690cda4f799c050d21df6c Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Sat, 28 Dec 2024 13:58:55 +0100
Subject: [PATCH 03/15] Density calibration tuning optimized.

---
 NAV_Algorithms/NAV_tuning_parameters.h  |  1 -
 NAV_Algorithms/air_density_observer.cpp | 55 +++++++++++++------------
 NAV_Algorithms/air_density_observer.h   | 23 +++++++----
 NAV_Algorithms/atmosphere.h             | 24 +++++------
 NAV_Algorithms/navigator.h              |  7 ++--
 NAV_Algorithms/organizer.h              |  8 +---
 6 files changed, 62 insertions(+), 56 deletions(-)

diff --git a/NAV_Algorithms/NAV_tuning_parameters.h b/NAV_Algorithms/NAV_tuning_parameters.h
index 0e76ae2..576dc91 100644
--- a/NAV_Algorithms/NAV_tuning_parameters.h
+++ b/NAV_Algorithms/NAV_tuning_parameters.h
@@ -63,7 +63,6 @@
 #define INDUCTION_STD_DEVIATION_LIMIT	0.03 	//!< results outperforming this number will be used further on
 
 #define AIRBORNE_TRIGGER_SPEED		0.5f //!< speed-compensator vario value m/s
-#define AIR_DENSITY_LETHARGY_FACTOR 	0.85
 
 #define MAG_SCALE			10000.0f //!< scale factor for high-precision integer statistics
 #define FAST_SAMPLING_REQUENCY 		100.0f
diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp
index 16b3acf..d347302 100644
--- a/NAV_Algorithms/air_density_observer.cpp
+++ b/NAV_Algorithms/air_density_observer.cpp
@@ -29,37 +29,42 @@ air_data_result air_density_observer::feed_metering( float pressure, float GNSS_
 {
   air_data_result air_data;
 
-#if DENSITY_MEASURMENT_COLLECTS_INTEGER
+  pressure_decimation_filter.respond( pressure);
+  altitude_decimation_filter.respond( GNSS_altitude);
+  --decimation_counter;
+  if( decimation_counter > 0)
+    return air_data;
+  decimation_counter = 20;
+
   density_QFF_calculator.add_value( GNSS_altitude * 100.0f, pressure);
-#else
-  // use values normalized around 1.0
-  density_QFF_calculator.add_value( GNSS_altitude * 1e-3, pressure * 1e-5);
-#endif
 
+  // update elevation range
   if( GNSS_altitude > max_altitude)
     max_altitude = GNSS_altitude;
 
   if( GNSS_altitude < min_altitude)
     min_altitude = GNSS_altitude;
 
-  if( ((max_altitude - min_altitude) < 2 * MINIMUM_ALTITUDE_RANGE) &&
+  // elevation range triggering
+  if( (max_altitude - min_altitude < MAXIMUM_ALTITUDE_RANGE) &&
       false == altitude_trigger.process(GNSS_altitude))
     return air_data;
 
-  if( ((max_altitude - min_altitude) < MINIMUM_ALTITUDE_RANGE) // ... forget this measurement
-    || (density_QFF_calculator.get_count() < 3000))
-    {
-      max_altitude = min_altitude = GNSS_altitude;
-      density_QFF_calculator.reset();
-      return air_data;
-    }
+  // if range too low: continue
+  if( (max_altitude - min_altitude) < MINIMUM_ALTITUDE_RANGE)
+    return air_data;
 
+  // if data points too rare: continue
+  if (density_QFF_calculator.get_count() < 100)
+    return air_data;
+
+  // process last acquisition phase data
   linear_least_square_result<evaluation_type> result;
   density_QFF_calculator.evaluate(result);
 
-//  Due to numeric effects, when using float the
-//  variance has been observed to be negative in some cases !
-//  Therefore using float this test can not be used.
+//  Due to numeric effects, the variance has been observed
+//  to be negative in some cases.
+//  If this is the case: Throw away this result.
   if( ( result.variance_slope < 0) || ( result.variance_offset < 0))
       {
       density_QFF_calculator.reset();
@@ -68,18 +73,16 @@ air_data_result air_density_observer::feed_metering( float pressure, float GNSS_
       }
 
  if( (result.variance_slope < MAX_ALLOWED_SLOPE_VARIANCE) &&
-     (result.variance_offset < MAX_ALLOWED_OFFSET_VARIANCE))
+     (result.variance_offset < MAX_ALLOWED_OFFSET_VARIANCE) )
     {
       air_data.QFF = (float)(result.y_offset);
-      float density = 100.0f * (float)(result.slope) * -0.10194f; // div by -9.81f;
-
-#if DENSITY_MEASURMENT_COLLECTS_INTEGER
-      float mean_altitude = density_QFF_calculator.get_mean_x() * 0.01f;
-#else
-      float pressure = 1e5 * density_QFF_calculator.get_mean_y();
-#endif
-      float ICAO_density_from_altitude = 1.2250 + mean_altitude * -1.1659e-4 + mean_altitude * mean_altitude * 3.786e-9;
-      air_data.density_correction = density / ICAO_density_from_altitude;
+      float density = 100.0f * (float)(result.slope) * -0.101936f; // div by -9.81f;
+
+      float reference_altitude = density_QFF_calculator.get_mean_x() * 0.01f;
+      float std_density =
+	  reference_altitude * reference_altitude *   0.000000003547494f
+	  -0.000115412739613f * reference_altitude +1.224096628212817f;
+      air_data.density_correction = density / std_density;
       air_data.valid = true;
     }
 
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index c4907a7..b920042 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -25,19 +25,22 @@
 #ifndef AIR_DENSITY_OBSERVER_H_
 #define AIR_DENSITY_OBSERVER_H_
 
+#include "pt2.h"
 #include "Linear_Least_Square_Fit.h"
 #include "trigger.h"
 
-#define DENSITY_MEASURMENT_COLLECTS_INTEGER 1
 typedef double evaluation_type;
 typedef uint64_t measurement_type;
 
-#define MAX_ALLOWED_SLOPE_VARIANCE 1e-9f
-#define MAX_ALLOWED_OFFSET_VARIANCE 5.0f
-#define MINIMUM_ALTITUDE_RANGE	400.0f
 #define ALTITUDE_TRIGGER_HYSTERESIS 50.0f
+#define MAX_ALLOWED_SLOPE_VARIANCE	3e-9
+#define MAX_ALLOWED_OFFSET_VARIANCE	150
+#define MINIMUM_ALTITUDE_RANGE		300.0f
+#define MAXIMUM_ALTITUDE_RANGE		500.0f
+#define ALTITUDE_TRIGGER_HYSTERESIS	50.0f
+#define AIR_DENSITY_LETHARGY	0.8f
 
-//! this class maintains offset and slope of the air density measurement
+//! Maintains offset and slope of the air density measurement
 class air_data_result
 {
 public:
@@ -51,14 +54,17 @@ class air_data_result
   bool valid;
 };
 
-//! this class measures air density and QFF
+//! Measures air density and reference pressure
 class air_density_observer
 {
 public:
   air_density_observer (void)
   : min_altitude(10000.0f),
     max_altitude(0.0f),
-    altitude_trigger( ALTITUDE_TRIGGER_HYSTERESIS)
+    altitude_trigger( ALTITUDE_TRIGGER_HYSTERESIS),
+    decimation_counter( 20),
+    altitude_decimation_filter( 0.025),
+    pressure_decimation_filter( 0.025)
   {
   }
   air_data_result feed_metering( float pressure, float MSL_altitude);
@@ -76,6 +82,9 @@ class air_density_observer
     float min_altitude;
     float max_altitude;
     trigger altitude_trigger;
+    unsigned decimation_counter;
+    pt2 <float, float> altitude_decimation_filter;
+    pt2 <float, float> pressure_decimation_filter;
 };
 
 #endif /* AIR_DENSITY_OBSERVER_H_ */
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index 7e40970..a38d137 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -29,6 +29,7 @@
 #include "embedded_math.h"
 #include <air_density_observer.h>
 #include <NAV_tuning_parameters.h>
+#include "NAV_tuning_parameters.h"
 #include <pt2.h>
 
 #define RECIP_STD_DENSITY_TIMES_2 1.632f
@@ -42,7 +43,7 @@
 /*! The offest for the conversion from degree celsius to kelvin */
 #define CELSIUS_TO_KELVIN_OFFSET 273.15f
 
-//! this class maintains instant atmosphere data like pressure, density etc
+//! Maintenance of atmosphere data like pressure, density etc.
 class atmosphere_t
 {
 public:
@@ -58,15 +59,6 @@ class atmosphere_t
   {
     density_correction_averager.settle(1.0f);
   }
-
-  //! set density to ICAO std density at present altitude
-  void normalize_density_correction( float GNSS_altitude)
-  {
-    float density_from_pressure = 1.0496346613e-5f * pressure + 0.1671546011f;
-    float ICAO_density_from_altitude = 1.2250 + GNSS_altitude * -1.1659e-4 + GNSS_altitude * GNSS_altitude * 3.786e-9;
-    density_correction = ICAO_density_from_altitude / density_from_pressure;
-    density_correction_averager.settle(density_correction);
-  }
   void update_density_correction( void)
   {
     density_correction_averager.respond(density_correction);
@@ -83,6 +75,14 @@ class atmosphere_t
   {
     return pressure;
   }
+  void normalize_density_correction_averager( float GNSS_altitude, float static_pressure)
+  {
+    float std_density =
+	0.000000003547494f * GNSS_altitude * GNSS_altitude
+	-0.000115412739613f * GNSS_altitude + 1.224096628212817f;
+    float pressure_density = 1.0496346613e-5f * pressure + 0.1671546011f;
+    density_correction_averager.settle( std_density / pressure_density);
+  }
   float get_density( void) const
   {
     return  (1.0496346613e-5f * pressure + 0.1671546011f) * density_correction_averager.get_output();
@@ -121,8 +121,8 @@ class atmosphere_t
     air_data_result result = density_QFF_calculator.feed_metering( pressure, MSL_altitude);
       if( result.valid)
 	{
-	  QFF = QFF * AIR_DENSITY_LETHARGY_FACTOR + (1.0f - AIR_DENSITY_LETHARGY_FACTOR) * result.QFF;
-	  density_correction = density_correction * AIR_DENSITY_LETHARGY_FACTOR + (1.0f - AIR_DENSITY_LETHARGY_FACTOR) * result.density_correction;
+	  QFF = QFF * AIR_DENSITY_LETHARGY + result.QFF * (1 - AIR_DENSITY_LETHARGY);
+	  density_correction = density_correction * AIR_DENSITY_LETHARGY + result.density_correction * (1 - AIR_DENSITY_LETHARGY);
 	}
     }
 
diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
index f253521..0fffd96 100644
--- a/NAV_Algorithms/navigator.h
+++ b/NAV_Algorithms/navigator.h
@@ -74,7 +74,10 @@ class navigator_t
     ahrs_magnetic.update_magnetic_induction_data( declination, inclination);
 #endif
   }
-
+  void normalize_density_correction( float GNSS_altitude, float static_pressure)
+  {
+    atmosphere.normalize_density_correction_averager(GNSS_altitude, static_pressure);
+  }
   void set_density_data( float temperature, float humidity)
   {
     if( ! isnan( temperature) && ! isnan( humidity) )
@@ -119,8 +122,6 @@ class navigator_t
 
   void reset_altitude( void)
   {
-    // set density correction to the value that will let GNSS altitude match pressure altitude
-    atmosphere.normalize_density_correction( -GNSS_negative_altitude);
     flight_observer.reset( atmosphere.get_negative_pressure_altitude(), GNSS_negative_altitude);
   }
   /**
diff --git a/NAV_Algorithms/organizer.h b/NAV_Algorithms/organizer.h
index 73efd95..d536e34 100644
--- a/NAV_Algorithms/organizer.h
+++ b/NAV_Algorithms/organizer.h
@@ -69,6 +69,7 @@ class organizer_t
 
   void initialize_after_first_measurement( output_data_t & output_data)
   {
+    navigator.normalize_density_correction( - output_data.c.position[DOWN], output_data.m.static_pressure);
     navigator.update_pressure( output_data.m.static_pressure - QNH_offset);
     navigator.initialize_QFF_density_metering( -output_data.c.position[DOWN]);
     navigator.reset_altitude ();
@@ -115,16 +116,9 @@ class organizer_t
 
   void update_every_10ms( output_data_t & output_data)
   {
-    // rotate sensor coordinates into airframe coordinates
-#if USE_LOWCOST_IMU == 1
-    acc  = sensor_mapping * output_data.m.lowcost_acc;
-    mag  = sensor_mapping * output_data.m.lowcost_mag;
-    gyro = sensor_mapping * output_data.m.lowcost_gyro;
-#else
     acc  = sensor_mapping * output_data.m.acc;
     mag  = sensor_mapping * output_data.m.mag;
     gyro = sensor_mapping * output_data.m.gyro;
-#endif
 
 #if DEVELOPMENT_ADDITIONS
     output_data.body_acc  = acc;

From af0c416ae378f2de15df9a2832d8ff5f8ee0a6db Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Sat, 28 Dec 2024 16:16:54 +0100
Subject: [PATCH 04/15] Air density mainainance reworked.

Tested in SIL: OK.
---
 NAV_Algorithms/air_density_observer.cpp |  2 +-
 NAV_Algorithms/air_density_observer.h   |  4 +-
 NAV_Algorithms/atmosphere.h             | 51 ++++++++++++++++---------
 NAV_Algorithms/navigator.cpp            |  4 +-
 NAV_Algorithms/navigator.h              |  2 +-
 5 files changed, 40 insertions(+), 23 deletions(-)

diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp
index d347302..a067a90 100644
--- a/NAV_Algorithms/air_density_observer.cpp
+++ b/NAV_Algorithms/air_density_observer.cpp
@@ -25,7 +25,7 @@
 #include "embedded_math.h"
 #include <air_density_observer.h>
 
-air_data_result air_density_observer::feed_metering( float pressure, float GNSS_altitude)
+air_data_result air_density_observer_t::feed_metering( float pressure, float GNSS_altitude)
 {
   air_data_result air_data;
 
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index b920042..11b9455 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -55,10 +55,10 @@ class air_data_result
 };
 
 //! Measures air density and reference pressure
-class air_density_observer
+class air_density_observer_t
 {
 public:
-  air_density_observer (void)
+  air_density_observer_t (void)
   : min_altitude(10000.0f),
     max_altitude(0.0f),
     altitude_trigger( ALTITUDE_TRIGGER_HYSTERESIS),
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index a38d137..21685de 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -54,18 +54,24 @@ class atmosphere_t
     temperature(20.0f),
     humidity( 0.0f),
     density_correction(1.0f),
-    density_correction_averager(0.001f),
-    QFF(101325)
+    extrapolated_sea_level_pressure(101325),
+    GNSS_altitude_based_density_available(false),
+    GNSS_altitude_based_density(1.2255f)
   {
-    density_correction_averager.settle(1.0f);
   }
-  void update_density_correction( void)
+  void update_density( float GNSS_altitude, bool valid)
   {
-    density_correction_averager.respond(density_correction);
+    if( valid)
+      {
+	GNSS_altitude_based_density = get_std_density( GNSS_altitude) * density_correction;
+	GNSS_altitude_based_density_available = true;
+      }
+    else
+      GNSS_altitude_based_density_available = false;
   }
   void initialize( float altitude)
   {
-    density_QFF_calculator.initialize(altitude);
+    air_density_observer.initialize(altitude);
   }
   void set_pressure( float p_abs)
   {
@@ -75,17 +81,27 @@ class atmosphere_t
   {
     return pressure;
   }
-  void normalize_density_correction_averager( float GNSS_altitude, float static_pressure)
+  float get_std_density( float GNSS_altitude)
   {
     float std_density =
 	0.000000003547494f * GNSS_altitude * GNSS_altitude
 	-0.000115412739613f * GNSS_altitude + 1.224096628212817f;
-    float pressure_density = 1.0496346613e-5f * pressure + 0.1671546011f;
-    density_correction_averager.settle( std_density / pressure_density);
+    return std_density;
+  }
+  float get_pressure_density( float static_pressure)
+  {
+    return 1.0496346613e-5f * static_pressure + 0.1671546011f;
+  }
+  void normalize_density_correction( float GNSS_altitude, float static_pressure)
+  {
+    density_correction = get_std_density(GNSS_altitude) / get_pressure_density(static_pressure);
   }
   float get_density( void) const
   {
-    return  (1.0496346613e-5f * pressure + 0.1671546011f) * density_correction_averager.get_output();
+    if( GNSS_altitude_based_density_available)
+      return GNSS_altitude_based_density;
+    else
+      return  (1.0496346613e-5f * pressure + 0.1671546011f) * density_correction;
   }
   float get_negative_pressure_altitude( void) const
   {
@@ -111,17 +127,17 @@ class atmosphere_t
     have_ambient_air_data = false;
   }
 
-  float get_QFF () const
+  float get_extrapolated_sea_level_pressure () const
   {
-    return QFF;
+    return extrapolated_sea_level_pressure;
   }
 
   void feed_QFF_density_metering( float pressure, float MSL_altitude)
     {
-    air_data_result result = density_QFF_calculator.feed_metering( pressure, MSL_altitude);
+    air_data_result result = air_density_observer.feed_metering( pressure, MSL_altitude);
       if( result.valid)
 	{
-	  QFF = QFF * AIR_DENSITY_LETHARGY + result.QFF * (1 - AIR_DENSITY_LETHARGY);
+	  extrapolated_sea_level_pressure = extrapolated_sea_level_pressure * AIR_DENSITY_LETHARGY + result.QFF * (1 - AIR_DENSITY_LETHARGY);
 	  density_correction = density_correction * AIR_DENSITY_LETHARGY + result.density_correction * (1 - AIR_DENSITY_LETHARGY);
 	}
     }
@@ -137,9 +153,10 @@ class atmosphere_t
   float temperature;
   float humidity;
   float density_correction;
-  pt2<float,float> density_correction_averager;
-  float QFF;
-  air_density_observer density_QFF_calculator;
+  float extrapolated_sea_level_pressure;
+  air_density_observer_t air_density_observer;
+  bool GNSS_altitude_based_density_available;
+  float GNSS_altitude_based_density;
 };
 
 #endif /* APPLICATION_ATMOSPHERE_H_ */
diff --git a/NAV_Algorithms/navigator.cpp b/NAV_Algorithms/navigator.cpp
index 30cb7ec..5348f0d 100644
--- a/NAV_Algorithms/navigator.cpp
+++ b/NAV_Algorithms/navigator.cpp
@@ -89,7 +89,7 @@ bool navigator_t::update_at_10Hz ()
 	air_pressure_resampler_100Hz_10Hz.get_output(),
 	flight_observer.get_filtered_GNSS_altitude());
 
-  atmosphere.update_density_correction(); // here because of the 10 Hz call frequency
+  atmosphere.update_density( -GNSS_negative_altitude, GNSS_fix_type > 0);
 
   wind_observer.process_at_10_Hz( ahrs);
 
@@ -140,7 +140,7 @@ void navigator_t::report_data( output_data_t &d)
 
 #if DEVELOPMENT_ADDITIONS
 
-    d.QFF			= atmosphere.get_QFF();
+    d.QFF			= atmosphere.get_extrapolated_sea_level_pressure();
     d.nav_correction		= ahrs.get_nav_correction();
     d.gyro_correction		= ahrs.get_gyro_correction();
     d.nav_acceleration_gnss 	= ahrs.get_nav_acceleration();
diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
index 0fffd96..0fd719e 100644
--- a/NAV_Algorithms/navigator.h
+++ b/NAV_Algorithms/navigator.h
@@ -76,7 +76,7 @@ class navigator_t
   }
   void normalize_density_correction( float GNSS_altitude, float static_pressure)
   {
-    atmosphere.normalize_density_correction_averager(GNSS_altitude, static_pressure);
+    atmosphere.normalize_density_correction( GNSS_altitude, static_pressure);
   }
   void set_density_data( float temperature, float humidity)
   {

From 4ccbd6fe6d63835ae50b4ba0f144d3cbe8d38569 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Sat, 28 Dec 2024 16:16:54 +0100
Subject: [PATCH 05/15] Air density maintainance reworked.

Tested in SIL: OK.
---
 NAV_Algorithms/air_density_observer.cpp |  2 +-
 NAV_Algorithms/air_density_observer.h   |  4 +-
 NAV_Algorithms/atmosphere.h             | 51 ++++++++++++++++---------
 NAV_Algorithms/navigator.cpp            |  4 +-
 NAV_Algorithms/navigator.h              |  2 +-
 5 files changed, 40 insertions(+), 23 deletions(-)

diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp
index d347302..a067a90 100644
--- a/NAV_Algorithms/air_density_observer.cpp
+++ b/NAV_Algorithms/air_density_observer.cpp
@@ -25,7 +25,7 @@
 #include "embedded_math.h"
 #include <air_density_observer.h>
 
-air_data_result air_density_observer::feed_metering( float pressure, float GNSS_altitude)
+air_data_result air_density_observer_t::feed_metering( float pressure, float GNSS_altitude)
 {
   air_data_result air_data;
 
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index b920042..11b9455 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -55,10 +55,10 @@ class air_data_result
 };
 
 //! Measures air density and reference pressure
-class air_density_observer
+class air_density_observer_t
 {
 public:
-  air_density_observer (void)
+  air_density_observer_t (void)
   : min_altitude(10000.0f),
     max_altitude(0.0f),
     altitude_trigger( ALTITUDE_TRIGGER_HYSTERESIS),
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index a38d137..21685de 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -54,18 +54,24 @@ class atmosphere_t
     temperature(20.0f),
     humidity( 0.0f),
     density_correction(1.0f),
-    density_correction_averager(0.001f),
-    QFF(101325)
+    extrapolated_sea_level_pressure(101325),
+    GNSS_altitude_based_density_available(false),
+    GNSS_altitude_based_density(1.2255f)
   {
-    density_correction_averager.settle(1.0f);
   }
-  void update_density_correction( void)
+  void update_density( float GNSS_altitude, bool valid)
   {
-    density_correction_averager.respond(density_correction);
+    if( valid)
+      {
+	GNSS_altitude_based_density = get_std_density( GNSS_altitude) * density_correction;
+	GNSS_altitude_based_density_available = true;
+      }
+    else
+      GNSS_altitude_based_density_available = false;
   }
   void initialize( float altitude)
   {
-    density_QFF_calculator.initialize(altitude);
+    air_density_observer.initialize(altitude);
   }
   void set_pressure( float p_abs)
   {
@@ -75,17 +81,27 @@ class atmosphere_t
   {
     return pressure;
   }
-  void normalize_density_correction_averager( float GNSS_altitude, float static_pressure)
+  float get_std_density( float GNSS_altitude)
   {
     float std_density =
 	0.000000003547494f * GNSS_altitude * GNSS_altitude
 	-0.000115412739613f * GNSS_altitude + 1.224096628212817f;
-    float pressure_density = 1.0496346613e-5f * pressure + 0.1671546011f;
-    density_correction_averager.settle( std_density / pressure_density);
+    return std_density;
+  }
+  float get_pressure_density( float static_pressure)
+  {
+    return 1.0496346613e-5f * static_pressure + 0.1671546011f;
+  }
+  void normalize_density_correction( float GNSS_altitude, float static_pressure)
+  {
+    density_correction = get_std_density(GNSS_altitude) / get_pressure_density(static_pressure);
   }
   float get_density( void) const
   {
-    return  (1.0496346613e-5f * pressure + 0.1671546011f) * density_correction_averager.get_output();
+    if( GNSS_altitude_based_density_available)
+      return GNSS_altitude_based_density;
+    else
+      return  (1.0496346613e-5f * pressure + 0.1671546011f) * density_correction;
   }
   float get_negative_pressure_altitude( void) const
   {
@@ -111,17 +127,17 @@ class atmosphere_t
     have_ambient_air_data = false;
   }
 
-  float get_QFF () const
+  float get_extrapolated_sea_level_pressure () const
   {
-    return QFF;
+    return extrapolated_sea_level_pressure;
   }
 
   void feed_QFF_density_metering( float pressure, float MSL_altitude)
     {
-    air_data_result result = density_QFF_calculator.feed_metering( pressure, MSL_altitude);
+    air_data_result result = air_density_observer.feed_metering( pressure, MSL_altitude);
       if( result.valid)
 	{
-	  QFF = QFF * AIR_DENSITY_LETHARGY + result.QFF * (1 - AIR_DENSITY_LETHARGY);
+	  extrapolated_sea_level_pressure = extrapolated_sea_level_pressure * AIR_DENSITY_LETHARGY + result.QFF * (1 - AIR_DENSITY_LETHARGY);
 	  density_correction = density_correction * AIR_DENSITY_LETHARGY + result.density_correction * (1 - AIR_DENSITY_LETHARGY);
 	}
     }
@@ -137,9 +153,10 @@ class atmosphere_t
   float temperature;
   float humidity;
   float density_correction;
-  pt2<float,float> density_correction_averager;
-  float QFF;
-  air_density_observer density_QFF_calculator;
+  float extrapolated_sea_level_pressure;
+  air_density_observer_t air_density_observer;
+  bool GNSS_altitude_based_density_available;
+  float GNSS_altitude_based_density;
 };
 
 #endif /* APPLICATION_ATMOSPHERE_H_ */
diff --git a/NAV_Algorithms/navigator.cpp b/NAV_Algorithms/navigator.cpp
index 30cb7ec..5348f0d 100644
--- a/NAV_Algorithms/navigator.cpp
+++ b/NAV_Algorithms/navigator.cpp
@@ -89,7 +89,7 @@ bool navigator_t::update_at_10Hz ()
 	air_pressure_resampler_100Hz_10Hz.get_output(),
 	flight_observer.get_filtered_GNSS_altitude());
 
-  atmosphere.update_density_correction(); // here because of the 10 Hz call frequency
+  atmosphere.update_density( -GNSS_negative_altitude, GNSS_fix_type > 0);
 
   wind_observer.process_at_10_Hz( ahrs);
 
@@ -140,7 +140,7 @@ void navigator_t::report_data( output_data_t &d)
 
 #if DEVELOPMENT_ADDITIONS
 
-    d.QFF			= atmosphere.get_QFF();
+    d.QFF			= atmosphere.get_extrapolated_sea_level_pressure();
     d.nav_correction		= ahrs.get_nav_correction();
     d.gyro_correction		= ahrs.get_gyro_correction();
     d.nav_acceleration_gnss 	= ahrs.get_nav_acceleration();
diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
index 0fffd96..0fd719e 100644
--- a/NAV_Algorithms/navigator.h
+++ b/NAV_Algorithms/navigator.h
@@ -76,7 +76,7 @@ class navigator_t
   }
   void normalize_density_correction( float GNSS_altitude, float static_pressure)
   {
-    atmosphere.normalize_density_correction_averager(GNSS_altitude, static_pressure);
+    atmosphere.normalize_density_correction( GNSS_altitude, static_pressure);
   }
   void set_density_data( float temperature, float humidity)
   {

From 2c20e63e7251794380a2405df4140e6600b8cc65 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Sat, 28 Dec 2024 18:15:56 +0100
Subject: [PATCH 06/15] Merge branch 'density_mechanism_optimized' of
 git@github.com:larus-breeze/sw_sensor_algorithms.git into
 density_mechanism_optimized

---
 NAV_Algorithms/air_density_observer.cpp | 8 ++++----
 NAV_Algorithms/air_density_observer.h   | 4 +---
 2 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp
index a067a90..bb2cd1f 100644
--- a/NAV_Algorithms/air_density_observer.cpp
+++ b/NAV_Algorithms/air_density_observer.cpp
@@ -45,15 +45,15 @@ air_data_result air_density_observer_t::feed_metering( float pressure, float GNS
   if( GNSS_altitude < min_altitude)
     min_altitude = GNSS_altitude;
 
+  // if range too low: continue
+  if( (max_altitude - min_altitude) < MINIMUM_ALTITUDE_RANGE)
+    return air_data;
+
   // elevation range triggering
   if( (max_altitude - min_altitude < MAXIMUM_ALTITUDE_RANGE) &&
       false == altitude_trigger.process(GNSS_altitude))
     return air_data;
 
-  // if range too low: continue
-  if( (max_altitude - min_altitude) < MINIMUM_ALTITUDE_RANGE)
-    return air_data;
-
   // if data points too rare: continue
   if (density_QFF_calculator.get_count() < 100)
     return air_data;
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index 11b9455..85a2dcc 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -33,12 +33,10 @@ typedef double evaluation_type;
 typedef uint64_t measurement_type;
 
 #define ALTITUDE_TRIGGER_HYSTERESIS 50.0f
-#define MAX_ALLOWED_SLOPE_VARIANCE	3e-9
+#define MAX_ALLOWED_SLOPE_VARIANCE	1e-8
 #define MAX_ALLOWED_OFFSET_VARIANCE	150
 #define MINIMUM_ALTITUDE_RANGE		300.0f
 #define MAXIMUM_ALTITUDE_RANGE		500.0f
-#define ALTITUDE_TRIGGER_HYSTERESIS	50.0f
-#define AIR_DENSITY_LETHARGY	0.8f
 
 //! Maintains offset and slope of the air density measurement
 class air_data_result

From e5d0ff194d7d545cfbde8b5f9b613a779b039bd1 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Sat, 28 Dec 2024 18:48:15 +0100
Subject: [PATCH 07/15] Anti-GIT-fixes

---
 NAV_Algorithms/NAV_tuning_parameters.h | 1 +
 NAV_Algorithms/atmosphere.h            | 1 -
 2 files changed, 1 insertion(+), 1 deletion(-)

diff --git a/NAV_Algorithms/NAV_tuning_parameters.h b/NAV_Algorithms/NAV_tuning_parameters.h
index 576dc91..5db83ab 100644
--- a/NAV_Algorithms/NAV_tuning_parameters.h
+++ b/NAV_Algorithms/NAV_tuning_parameters.h
@@ -63,6 +63,7 @@
 #define INDUCTION_STD_DEVIATION_LIMIT	0.03 	//!< results outperforming this number will be used further on
 
 #define AIRBORNE_TRIGGER_SPEED		0.5f //!< speed-compensator vario value m/s
+#define AIR_DENSITY_LETHARGY	 	0.85
 
 #define MAG_SCALE			10000.0f //!< scale factor for high-precision integer statistics
 #define FAST_SAMPLING_REQUENCY 		100.0f
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index 21685de..3df5b60 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -28,7 +28,6 @@
 
 #include "embedded_math.h"
 #include <air_density_observer.h>
-#include <NAV_tuning_parameters.h>
 #include "NAV_tuning_parameters.h"
 #include <pt2.h>
 

From 53869eea5b1af5b26c1d05f0096c456b0ef76846 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Mon, 30 Dec 2024 15:05:40 +0100
Subject: [PATCH 08/15] Density mechanism optimized

Introduced boxcar filter (2 samples)
---
 NAV_Algorithms/NAV_tuning_parameters.h |  1 +
 NAV_Algorithms/air_density_observer.h  |  8 ++++----
 NAV_Algorithms/atmosphere.h            | 23 +++++++++++++++++++++--
 NAV_Algorithms/navigator.cpp           |  2 +-
 NAV_Algorithms/navigator.h             |  2 +-
 5 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/NAV_Algorithms/NAV_tuning_parameters.h b/NAV_Algorithms/NAV_tuning_parameters.h
index 5db83ab..242dd86 100644
--- a/NAV_Algorithms/NAV_tuning_parameters.h
+++ b/NAV_Algorithms/NAV_tuning_parameters.h
@@ -63,6 +63,7 @@
 #define INDUCTION_STD_DEVIATION_LIMIT	0.03 	//!< results outperforming this number will be used further on
 
 #define AIRBORNE_TRIGGER_SPEED		0.5f //!< speed-compensator vario value m/s
+#define AIR_DENSITY_IIR_FILTER		0
 #define AIR_DENSITY_LETHARGY	 	0.85
 
 #define MAG_SCALE			10000.0f //!< scale factor for high-precision integer statistics
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index 85a2dcc..cbcea37 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -32,11 +32,11 @@
 typedef double evaluation_type;
 typedef uint64_t measurement_type;
 
-#define ALTITUDE_TRIGGER_HYSTERESIS 50.0f
-#define MAX_ALLOWED_SLOPE_VARIANCE	1e-8
-#define MAX_ALLOWED_OFFSET_VARIANCE	150
+#define ALTITUDE_TRIGGER_HYSTERESIS	50.0f
+#define MAX_ALLOWED_SLOPE_VARIANCE	3e-9
+#define MAX_ALLOWED_OFFSET_VARIANCE	200
 #define MINIMUM_ALTITUDE_RANGE		300.0f
-#define MAXIMUM_ALTITUDE_RANGE		500.0f
+#define MAXIMUM_ALTITUDE_RANGE		800.0f
 
 //! Maintains offset and slope of the air density measurement
 class air_data_result
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index 3df5b60..e27857d 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -55,7 +55,9 @@ class atmosphere_t
     density_correction(1.0f),
     extrapolated_sea_level_pressure(101325),
     GNSS_altitude_based_density_available(false),
-    GNSS_altitude_based_density(1.2255f)
+    GNSS_altitude_based_density(1.2255f),
+    old_qff( 0.0f),
+    old_density_correction( 0.0f)
   {
   }
   void update_density( float GNSS_altitude, bool valid)
@@ -131,13 +133,28 @@ class atmosphere_t
     return extrapolated_sea_level_pressure;
   }
 
-  void feed_QFF_density_metering( float pressure, float MSL_altitude)
+  void air_density_metering( float pressure, float MSL_altitude)
     {
     air_data_result result = air_density_observer.feed_metering( pressure, MSL_altitude);
       if( result.valid)
 	{
+#if AIR_DENSITY_IIR_FILTER
 	  extrapolated_sea_level_pressure = extrapolated_sea_level_pressure * AIR_DENSITY_LETHARGY + result.QFF * (1 - AIR_DENSITY_LETHARGY);
 	  density_correction = density_correction * AIR_DENSITY_LETHARGY + result.density_correction * (1 - AIR_DENSITY_LETHARGY);
+#else
+	  if( old_density_correction == 0.0f)
+	    {
+		old_qff = extrapolated_sea_level_pressure = result.QFF;
+		old_density_correction = density_correction = result.density_correction;
+	    }
+	  else
+	    {
+		  extrapolated_sea_level_pressure = (result.QFF + old_qff) * 0.5f;
+		  old_qff = result.QFF;
+		  density_correction = (result.density_correction + old_density_correction) * 0.5f;
+		  old_density_correction = result.density_correction;
+	    }
+#endif
 	}
     }
 
@@ -156,6 +173,8 @@ class atmosphere_t
   air_density_observer_t air_density_observer;
   bool GNSS_altitude_based_density_available;
   float GNSS_altitude_based_density;
+  float old_qff;
+  float old_density_correction;
 };
 
 #endif /* APPLICATION_ATMOSPHERE_H_ */
diff --git a/NAV_Algorithms/navigator.cpp b/NAV_Algorithms/navigator.cpp
index 5348f0d..e760e0d 100644
--- a/NAV_Algorithms/navigator.cpp
+++ b/NAV_Algorithms/navigator.cpp
@@ -85,7 +85,7 @@ void navigator_t::update_GNSS_data( const coordinates_t &coordinates)
 bool navigator_t::update_at_10Hz ()
 {
   bool landing_detected=false;
-  atmosphere.feed_QFF_density_metering(
+  atmosphere.air_density_metering(
 	air_pressure_resampler_100Hz_10Hz.get_output(),
 	flight_observer.get_filtered_GNSS_altitude());
 
diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
index 0fd719e..00dee8c 100644
--- a/NAV_Algorithms/navigator.h
+++ b/NAV_Algorithms/navigator.h
@@ -92,7 +92,7 @@ class navigator_t
 
   void feed_QFF_density_metering( float pressure, float MSL_altitude)
   {
-    atmosphere.feed_QFF_density_metering( pressure, MSL_altitude);
+    atmosphere.air_density_metering( pressure, MSL_altitude);
   }
 
   void disregard_density_data( void)

From df4344f62d1057a2f6263ff4f73e1baa1957707f Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Thu, 2 Jan 2025 17:34:03 +0100
Subject: [PATCH 09/15] Air density initialization simplified and optimized.

---
 NAV_Algorithms/atmosphere.h | 4 ----
 NAV_Algorithms/navigator.h  | 4 ----
 NAV_Algorithms/organizer.h  | 1 -
 3 files changed, 9 deletions(-)

diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index e27857d..3b03ba2 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -93,10 +93,6 @@ class atmosphere_t
   {
     return 1.0496346613e-5f * static_pressure + 0.1671546011f;
   }
-  void normalize_density_correction( float GNSS_altitude, float static_pressure)
-  {
-    density_correction = get_std_density(GNSS_altitude) / get_pressure_density(static_pressure);
-  }
   float get_density( void) const
   {
     if( GNSS_altitude_based_density_available)
diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
index 00dee8c..9f684fa 100644
--- a/NAV_Algorithms/navigator.h
+++ b/NAV_Algorithms/navigator.h
@@ -74,10 +74,6 @@ class navigator_t
     ahrs_magnetic.update_magnetic_induction_data( declination, inclination);
 #endif
   }
-  void normalize_density_correction( float GNSS_altitude, float static_pressure)
-  {
-    atmosphere.normalize_density_correction( GNSS_altitude, static_pressure);
-  }
   void set_density_data( float temperature, float humidity)
   {
     if( ! isnan( temperature) && ! isnan( humidity) )
diff --git a/NAV_Algorithms/organizer.h b/NAV_Algorithms/organizer.h
index d536e34..e6a0643 100644
--- a/NAV_Algorithms/organizer.h
+++ b/NAV_Algorithms/organizer.h
@@ -69,7 +69,6 @@ class organizer_t
 
   void initialize_after_first_measurement( output_data_t & output_data)
   {
-    navigator.normalize_density_correction( - output_data.c.position[DOWN], output_data.m.static_pressure);
     navigator.update_pressure( output_data.m.static_pressure - QNH_offset);
     navigator.initialize_QFF_density_metering( -output_data.c.position[DOWN]);
     navigator.reset_altitude ();

From 1df64750c13cb77aecca731be3db3e14a7aa7493 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Fri, 3 Jan 2025 13:58:27 +0100
Subject: [PATCH 10/15] Density mechanism simplified.

Using statistically weighed day average now.
---
 NAV_Algorithms/air_density_observer.cpp |  1 +
 NAV_Algorithms/air_density_observer.h   |  2 ++
 NAV_Algorithms/atmosphere.h             | 27 +++++++------------------
 3 files changed, 10 insertions(+), 20 deletions(-)

diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp
index bb2cd1f..4b39ff4 100644
--- a/NAV_Algorithms/air_density_observer.cpp
+++ b/NAV_Algorithms/air_density_observer.cpp
@@ -83,6 +83,7 @@ air_data_result air_density_observer_t::feed_metering( float pressure, float GNS
 	  reference_altitude * reference_altitude *   0.000000003547494f
 	  -0.000115412739613f * reference_altitude +1.224096628212817f;
       air_data.density_correction = density / std_density;
+      air_data.density_variance = result.variance_slope;
       air_data.valid = true;
     }
 
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index cbcea37..6208083 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -44,10 +44,12 @@ class air_data_result
 public:
   air_data_result( void)
     : density_correction(1.0f),
+      density_variance(1.0f),
       QFF(101325.0f),
       valid( false)
   {}
   float density_correction;
+  float density_variance;
   float QFF;
   bool valid;
 };
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index 3b03ba2..5b0c786 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -56,8 +56,8 @@ class atmosphere_t
     extrapolated_sea_level_pressure(101325),
     GNSS_altitude_based_density_available(false),
     GNSS_altitude_based_density(1.2255f),
-    old_qff( 0.0f),
-    old_density_correction( 0.0f)
+    weight_sum(0.0f),
+    density_factor_weighed_sum(0.0f)
   {
   }
   void update_density( float GNSS_altitude, bool valid)
@@ -134,23 +134,9 @@ class atmosphere_t
     air_data_result result = air_density_observer.feed_metering( pressure, MSL_altitude);
       if( result.valid)
 	{
-#if AIR_DENSITY_IIR_FILTER
-	  extrapolated_sea_level_pressure = extrapolated_sea_level_pressure * AIR_DENSITY_LETHARGY + result.QFF * (1 - AIR_DENSITY_LETHARGY);
-	  density_correction = density_correction * AIR_DENSITY_LETHARGY + result.density_correction * (1 - AIR_DENSITY_LETHARGY);
-#else
-	  if( old_density_correction == 0.0f)
-	    {
-		old_qff = extrapolated_sea_level_pressure = result.QFF;
-		old_density_correction = density_correction = result.density_correction;
-	    }
-	  else
-	    {
-		  extrapolated_sea_level_pressure = (result.QFF + old_qff) * 0.5f;
-		  old_qff = result.QFF;
-		  density_correction = (result.density_correction + old_density_correction) * 0.5f;
-		  old_density_correction = result.density_correction;
-	    }
-#endif
+	  weight_sum +=  1.0f / result.density_variance;
+	  density_factor_weighed_sum += result.density_correction / result.density_variance;
+	  density_correction = density_factor_weighed_sum / weight_sum;
 	}
     }
 
@@ -169,8 +155,9 @@ class atmosphere_t
   air_density_observer_t air_density_observer;
   bool GNSS_altitude_based_density_available;
   float GNSS_altitude_based_density;
-  float old_qff;
   float old_density_correction;
+  float weight_sum;
+  float density_factor_weighed_sum;
 };
 
 #endif /* APPLICATION_ATMOSPHERE_H_ */

From 784de05cdb4a416a3d44372a95011e3b8f4323b4 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Fri, 3 Jan 2025 17:11:09 +0100
Subject: [PATCH 11/15] Adaptive air density mechanism.

Measurement variance honored.
Decay of older results.

EXPERIMENTAL !
---
 NAV_Algorithms/NAV_tuning_parameters.h | 2 --
 NAV_Algorithms/air_density_observer.h  | 2 ++
 NAV_Algorithms/atmosphere.h            | 8 ++++++--
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/NAV_Algorithms/NAV_tuning_parameters.h b/NAV_Algorithms/NAV_tuning_parameters.h
index 242dd86..576dc91 100644
--- a/NAV_Algorithms/NAV_tuning_parameters.h
+++ b/NAV_Algorithms/NAV_tuning_parameters.h
@@ -63,8 +63,6 @@
 #define INDUCTION_STD_DEVIATION_LIMIT	0.03 	//!< results outperforming this number will be used further on
 
 #define AIRBORNE_TRIGGER_SPEED		0.5f //!< speed-compensator vario value m/s
-#define AIR_DENSITY_IIR_FILTER		0
-#define AIR_DENSITY_LETHARGY	 	0.85
 
 #define MAG_SCALE			10000.0f //!< scale factor for high-precision integer statistics
 #define FAST_SAMPLING_REQUENCY 		100.0f
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index 6208083..2f33de8 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -37,6 +37,8 @@ typedef uint64_t measurement_type;
 #define MAX_ALLOWED_OFFSET_VARIANCE	200
 #define MINIMUM_ALTITUDE_RANGE		300.0f
 #define MAXIMUM_ALTITUDE_RANGE		800.0f
+#define USE_AIR_DENSITY_LETHARGY	1
+#define AIR_DENSITY_LETHARGY 		0.7
 
 //! Maintains offset and slope of the air density measurement
 class air_data_result
diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index 5b0c786..c65c04c 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -134,9 +134,13 @@ class atmosphere_t
     air_data_result result = air_density_observer.feed_metering( pressure, MSL_altitude);
       if( result.valid)
 	{
-	  weight_sum +=  1.0f / result.density_variance;
-	  density_factor_weighed_sum += result.density_correction / result.density_variance;
+#if USE_AIR_DENSITY_LETHARGY
+	  weight_sum =  weight_sum * AIR_DENSITY_LETHARGY + (AIR_DENSITY_LETHARGY -1) / result.density_variance;
+	  density_factor_weighed_sum =  density_factor_weighed_sum * AIR_DENSITY_LETHARGY + (AIR_DENSITY_LETHARGY -1) * result.density_correction / result.density_variance;
 	  density_correction = density_factor_weighed_sum / weight_sum;
+#else
+	  density_correction = result.density_correction;
+#endif
 	}
     }
 

From d148c0a2d74359266f7024ca20192dfd349973c3 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Sun, 5 Jan 2025 14:42:44 +0100
Subject: [PATCH 12/15] Postpone density update until 2nd measurment is done.

---
 NAV_Algorithms/atmosphere.h | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h
index c65c04c..f5c37c6 100644
--- a/NAV_Algorithms/atmosphere.h
+++ b/NAV_Algorithms/atmosphere.h
@@ -135,9 +135,14 @@ class atmosphere_t
       if( result.valid)
 	{
 #if USE_AIR_DENSITY_LETHARGY
+	  bool first_measurement = (weight_sum == 0.0f);
+
 	  weight_sum =  weight_sum * AIR_DENSITY_LETHARGY + (AIR_DENSITY_LETHARGY -1) / result.density_variance;
 	  density_factor_weighed_sum =  density_factor_weighed_sum * AIR_DENSITY_LETHARGY + (AIR_DENSITY_LETHARGY -1) * result.density_correction / result.density_variance;
-	  density_correction = density_factor_weighed_sum / weight_sum;
+
+	  // postpone update unless we have two measurements
+	  if( ! first_measurement)
+	    density_correction = density_factor_weighed_sum / weight_sum;
 #else
 	  density_correction = result.density_correction;
 #endif

From 9a915ade57f1c326458e0eb3fe7ea43baab5bc06 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Mon, 6 Jan 2025 12:37:23 +0100
Subject: [PATCH 13/15] Bugfix: Air density measurement startet on takeoff.

Data decimation filter time constatnt optitmized.
SIL-tested: OK.
---
 NAV_Algorithms/air_density_observer.cpp |  2 +-
 NAV_Algorithms/air_density_observer.h   |  5 +++--
 NAV_Algorithms/airborne_detector.h      |  8 +++++++-
 NAV_Algorithms/navigator.cpp            | 10 +++++++---
 4 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp
index 4b39ff4..45d3d21 100644
--- a/NAV_Algorithms/air_density_observer.cpp
+++ b/NAV_Algorithms/air_density_observer.cpp
@@ -34,7 +34,7 @@ air_data_result air_density_observer_t::feed_metering( float pressure, float GNS
   --decimation_counter;
   if( decimation_counter > 0)
     return air_data;
-  decimation_counter = 20;
+  decimation_counter = AIR_DENSITY_DECIMATION;
 
   density_QFF_calculator.add_value( GNSS_altitude * 100.0f, pressure);
 
diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h
index 2f33de8..5f126f5 100644
--- a/NAV_Algorithms/air_density_observer.h
+++ b/NAV_Algorithms/air_density_observer.h
@@ -39,6 +39,7 @@ typedef uint64_t measurement_type;
 #define MAXIMUM_ALTITUDE_RANGE		800.0f
 #define USE_AIR_DENSITY_LETHARGY	1
 #define AIR_DENSITY_LETHARGY 		0.7
+#define AIR_DENSITY_DECIMATION		20
 
 //! Maintains offset and slope of the air density measurement
 class air_data_result
@@ -65,8 +66,8 @@ class air_density_observer_t
     max_altitude(0.0f),
     altitude_trigger( ALTITUDE_TRIGGER_HYSTERESIS),
     decimation_counter( 20),
-    altitude_decimation_filter( 0.025),
-    pressure_decimation_filter( 0.025)
+    altitude_decimation_filter( 1.0f / AIR_DENSITY_DECIMATION * 0.25f),
+    pressure_decimation_filter( 1.0f / AIR_DENSITY_DECIMATION * 0.25f)
   {
   }
   air_data_result feed_metering( float pressure, float MSL_altitude);
diff --git a/NAV_Algorithms/airborne_detector.h b/NAV_Algorithms/airborne_detector.h
index d3fb97e..39e716c 100644
--- a/NAV_Algorithms/airborne_detector.h
+++ b/NAV_Algorithms/airborne_detector.h
@@ -50,7 +50,13 @@ class airborne_detector_t
 	}
       }
   }
-    bool detect_just_landed( void)
+
+  bool is_airborne( void)
+    {
+      return airborne_counter > 0;
+    }
+
+  bool detect_just_landed( void)
     {
       if( just_landed)
 	{
diff --git a/NAV_Algorithms/navigator.cpp b/NAV_Algorithms/navigator.cpp
index e760e0d..cb27554 100644
--- a/NAV_Algorithms/navigator.cpp
+++ b/NAV_Algorithms/navigator.cpp
@@ -85,9 +85,6 @@ void navigator_t::update_GNSS_data( const coordinates_t &coordinates)
 bool navigator_t::update_at_10Hz ()
 {
   bool landing_detected=false;
-  atmosphere.air_density_metering(
-	air_pressure_resampler_100Hz_10Hz.get_output(),
-	flight_observer.get_filtered_GNSS_altitude());
 
   atmosphere.update_density( -GNSS_negative_altitude, GNSS_fix_type > 0);
 
@@ -103,6 +100,13 @@ bool navigator_t::update_at_10Hz ()
       ahrs.write_calibration_into_EEPROM();
       landing_detected = true;
     }
+
+  if( airborne_detector.is_airborne())
+    atmosphere.air_density_metering(
+	air_pressure_resampler_100Hz_10Hz.get_output(),
+	flight_observer.get_filtered_GNSS_altitude());
+
+
   return landing_detected;
 }
 

From 0bd215259416f642863ee3e32d50c3daff928b2f Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Mon, 6 Jan 2025 15:30:39 +0100
Subject: [PATCH 14/15] Tiny optimization: Pressure decimation filter tuned.

---
 NAV_Algorithms/navigator.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
index 9f684fa..15cf817 100644
--- a/NAV_Algorithms/navigator.h
+++ b/NAV_Algorithms/navigator.h
@@ -50,7 +50,7 @@ class navigator_t
 	 flight_observer(),
 	 wind_observer(),
 	 airborne_detector(),
-	 air_pressure_resampler_100Hz_10Hz(0.04f), // f / fcutoff = 80% * 0.5 * 0.1
+	 air_pressure_resampler_100Hz_10Hz(0.04f), // = 2.5 Hz @ 100Hz
 	 pitot_pressure(0.0f),
 	 TAS( 0.0f),
 	 IAS( 0.0f),

From 3cc6285ebbe28f309fe539e7f05044c1aa02de81 Mon Sep 17 00:00:00 2001
From: realtimepeople <klaus.schaefer@h-da.de>
Date: Mon, 6 Jan 2025 15:31:41 +0100
Subject: [PATCH 15/15] Tiny optimization: Pressure decimation filter tuned.

---
 NAV_Algorithms/navigator.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h
index 15cf817..815993f 100644
--- a/NAV_Algorithms/navigator.h
+++ b/NAV_Algorithms/navigator.h
@@ -50,7 +50,7 @@ class navigator_t
 	 flight_observer(),
 	 wind_observer(),
 	 airborne_detector(),
-	 air_pressure_resampler_100Hz_10Hz(0.04f), // = 2.5 Hz @ 100Hz
+	 air_pressure_resampler_100Hz_10Hz(0.025f), // = 2.5 Hz @ 100Hz
 	 pitot_pressure(0.0f),
 	 TAS( 0.0f),
 	 IAS( 0.0f),