[WIP] ekf2: EKF init immediately on first IMU sample

- tilt init use filtered accel/gyro on delayed time horizon - publish estimator_status on every EKF update (even if !_filter_initialised)
uxrce_dds_client: update cmake requirements to match Micro-XRCE-DDS-Client submodule
2026-05-25 13:07:34 +08:00 · 2024-11-27 17:22:51 -05:00 · 2024-11-27 14:09:13 -08:00 · 2024-11-27 13:51:10 -08:00 · 2024-11-27 16:42:13 -05:00 · 2024-11-27 16:42:13 -05:00
55 changed files with 776 additions and 844 deletions
@@ -25,8 +25,14 @@ jobs:
          submodules: false
          fetch-depth: 0

+      - name: Set PX4 Tag
+        id: px4-tag
+        run: |
+          echo "tag=$(git describe --tags --match 'v[0-9]*')" >> $GITHUB_OUTPUT
+
      - name: Login to Docker Hub
        uses: docker/login-action@v3
+        if: github.event_name != 'pull_request'
        with:
          username: ${{ secrets.DOCKERHUB_USERNAME }}
          password: ${{ secrets.DOCKERHUB_TOKEN }}
@@ -44,16 +50,15 @@ jobs:
        with:
          images: |
            ghcr.io/PX4/px4-dev
-            px4io/px4-dev
+            ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }}
          tags: |
-            type=schedule
            type=semver,pattern={{version}}
            type=semver,pattern={{major}}.{{minor}}
            type=semver,pattern={{major}}
-            type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}},priority=600
+            type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700
+            type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600
            type=ref,event=branch,suffix=,priority=500
            type=ref,event=pr
-            type=sha

      - name: Set up Docker Buildx
        uses: docker/setup-buildx-action@v3
@@ -45,7 +45,7 @@ param set-default CA_SV_CS1_TRQ_P 1
 param set-default CA_SV_CS1_TYPE 3
 param set-default CA_SV_CS2_TRQ_Y 1
 param set-default CA_SV_CS2_TYPE 4
-param set-default PWM_MAIN_FUNC3 201
-param set-default PWM_MAIN_FUNC4 202
-param set-default PWM_MAIN_FUNC5 203
-param set-default PWM_MAIN_FUNC6 101
+param set-default PWM_MAIN_FUNC1 201
+param set-default PWM_MAIN_FUNC2 202
+param set-default PWM_MAIN_FUNC3 203
+param set-default PWM_MAIN_FUNC4 101
@@ -43,7 +43,7 @@ fi

 # install git pre-commit hook
 HOOK_FILE="$DIR/../../.git/hooks/pre-commit"
-if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then
+if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ] && [ $- == *i* ]; then
 	echo ""
 	echo -e "\033[31mNinja tip: add a git pre-commit hook to automatically check code style\033[0m"
 	echo -e "Would you like to install one now? (\033[94mcp ./Tools/astyle/pre-commit .git/hooks/pre-commit\033[0m): [y/\033[1mN\033[0m]"
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,4 +31,10 @@
 #
 ############################################################################

-px4_add_library(pid pid.cpp)
+px4_add_library(PID
+	PID.cpp
+	PID.hpp
+)
+target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "PID.hpp"
+#include "lib/mathlib/math/Functions.hpp"
+
+void PID::setGains(const float P, const float I, const float D)
+{
+	_gain_proportional = P;
+	_gain_integral = I;
+	_gain_derivative = D;
+}
+
+float PID::update(const float feedback, const float dt, const bool update_integral)
+{
+	const float error = _setpoint - feedback;
+	const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt));
+
+	if (update_integral) {
+		updateIntegral(error, dt);
+	}
+
+	_last_feedback = feedback;
+	return math::constrain(output, -_limit_output, _limit_output);
+}
+
+void PID::updateIntegral(float error, const float dt)
+{
+	const float integral_new = _integral + _gain_integral * error * dt;
+
+	if (std::isfinite(integral_new)) {
+		_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
+	}
+}
+
+float PID::updateDerivative(float feedback, const float dt)
+{
+	float feedback_change = 0.f;
+
+	if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) {
+		feedback_change = (feedback - _last_feedback) / dt;
+	}
+
+	return feedback_change;
+}
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+class PID
+{
+public:
+	PID() = default;
+	virtual ~PID() = default;
+	void setOutputLimit(const float limit) { _limit_output = limit; }
+	void setIntegralLimit(const float limit) { _limit_integral = limit; }
+	void setGains(const float P, const float I, const float D);
+	void setSetpoint(const float setpoint) { _setpoint = setpoint; }
+	float update(const float feedback, const float dt, const bool update_integral = true);
+	float getIntegral() { return _integral; }
+	void resetIntegral() { _integral = 0.f; };
+	void resetDerivative() { _last_feedback = NAN; };
+private:
+	void updateIntegral(float error, const float dt);
+	float updateDerivative(float feedback, const float dt);
+
+	float _setpoint{0.f}; ///< current setpoint to track
+	float _integral{0.f}; ///< integral state
+	float _last_feedback{NAN};
+
+	// Gains, Limits
+	float _gain_proportional{0.f};
+	float _gain_integral{0.f};
+	float _gain_derivative{0.f};
+	float _limit_integral{0.f};
+	float _limit_output{0.f};
+};
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <PID.hpp>
+
+TEST(PIDTest, AllZeroCase)
+{
+	PID pid;
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+}
+
+TEST(PIDTest, OutputLimit)
+{
+	PID pid;
+	pid.setOutputLimit(.01f);
+	pid.setGains(.1f, 0.f, 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
+	EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
+	EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+	EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
+}
+
+TEST(PIDTest, ProportinalOnly)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(.1f, 0.f, 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+
+	float plant = 0.f;
+	float output = 10000.f;
+	int i; // need function scope to check how many steps
+
+	for (i = 1000; i > 0; i--) {
+		const float output_new = pid.update(plant, 0.f, false);
+		plant += output_new;
+
+		// expect the output to get smaller with each iteration
+		if (output_new >= output) {
+			break;
+		}
+
+		output = output_new;
+	}
+
+	EXPECT_FLOAT_EQ(plant, 1.f);
+	EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
+}
+
+TEST(PIDTest, InteralOpenLoop)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(0.f, .1f, 0.f);
+	pid.setIntegralLimit(.05f);
+	pid.setSetpoint(1.f);
+
+	// Zero error
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+
+	// Open loop ramp up
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+
+	// Open loop ramp down
+	pid.setSetpoint(-1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	pid.resetIntegral();
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
+}
@@ -1,185 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file pid.cpp
- *
- * Implementation of generic PID controller.
- *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#include "pid.h"
-#include <math.h>
-#include <px4_platform_common/defines.h>
-
-#define SIGMA 0.000001f
-
-__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
-{
-	pid->mode = mode;
-	pid->dt_min = dt_min;
-	pid->kp = 0.0f;
-	pid->ki = 0.0f;
-	pid->kd = 0.0f;
-	pid->integral = 0.0f;
-	pid->integral_limit = 0.0f;
-	pid->output_limit = 0.0f;
-	pid->error_previous = 0.0f;
-	pid->last_output = 0.0f;
-}
-
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
-{
-	int ret = 0;
-
-	if (PX4_ISFINITE(kp)) {
-		pid->kp = kp;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(ki)) {
-		pid->ki = ki;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(kd)) {
-		pid->kd = kd;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(integral_limit)) {
-		pid->integral_limit = integral_limit;
-
-	}  else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(output_limit)) {
-		pid->output_limit = output_limit;
-
-	}  else {
-		ret = 1;
-	}
-
-	return ret;
-}
-
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
-{
-	if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
-		return pid->last_output;
-	}
-
-	float i, d;
-
-	/* current error value */
-	float error = sp - val;
-
-	/* current error derivative */
-	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = error;
-
-	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
-		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = -val;
-
-	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
-		d = -val_dot;
-
-	} else {
-		d = 0.0f;
-	}
-
-	if (!PX4_ISFINITE(d)) {
-		d = 0.0f;
-	}
-
-	/* calculate PD output */
-	float output = (error * pid->kp) + (d * pid->kd);
-
-	if (pid->ki > SIGMA) {
-		// Calculate the error integral and check for saturation
-		i = pid->integral + (error * dt);
-
-		/* check for saturation */
-		if (PX4_ISFINITE(i)) {
-			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
-			    fabsf(i) <= pid->integral_limit) {
-				/* not saturated, use new integral value */
-				pid->integral = i;
-			}
-		}
-
-		/* add I component to output */
-		output += pid->integral * pid->ki;
-	}
-
-	/* limit output */
-	if (PX4_ISFINITE(output)) {
-		if (pid->output_limit > SIGMA) {
-			if (output > pid->output_limit) {
-				output = pid->output_limit;
-
-			} else if (output < -pid->output_limit) {
-				output = -pid->output_limit;
-			}
-		}
-
-		pid->last_output = output;
-	}
-
-	return pid->last_output;
-}
-
-
-__EXPORT void pid_reset_integral(PID_t *pid)
-{
-	pid->integral = 0.0f;
-}
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file pid.h
- *
- * Definition of generic PID controller.
- *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#ifndef PID_H_
-#define PID_H_
-
-#include <stdint.h>
-
-__BEGIN_DECLS
-
-typedef enum PID_MODE {
-	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
-	PID_MODE_DERIVATIV_NONE = 0,
-	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
-	 * val_dot in pid_calculate() will be ignored */
-	PID_MODE_DERIVATIV_CALC,
-	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
-	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
-	PID_MODE_DERIVATIV_CALC_NO_SP,
-	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-	PID_MODE_DERIVATIV_SET
-} pid_mode_t;
-
-typedef struct {
-	pid_mode_t mode;
-	float dt_min;
-	float kp;
-	float ki;
-	float kd;
-	float integral;
-	float integral_limit;
-	float output_limit;
-	float error_previous;
-	float last_output;
-} PID_t;
-
-__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
-__EXPORT void pid_reset_integral(PID_t *pid);
-
-__END_DECLS
-
-#endif /* PID_H_ */
@@ -43,35 +43,13 @@

 #include <mathlib/mathlib.h>

-bool Ekf::init(uint64_t timestamp)
-{
-	if (!_initialised) {
-		_initialised = initialise_interface(timestamp);
-		reset();
-	}
-
-	return _initialised;
-}
-
 void Ekf::reset()
 {
 	ECL_INFO("reset");

+	_state = {};
 	_state.quat_nominal.setIdentity();
-	_state.vel.setZero();
-	_state.pos.setZero();
-	_state.gyro_bias.setZero();
-	_state.accel_bias.setZero();

-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	_state.mag_I.setZero();
-	_state.mag_B.setZero();
-#endif // CONFIG_EKF2_MAGNETOMETER
-
-#if defined(CONFIG_EKF2_WIND)
-	_state.wind_vel.setZero();
-#endif // CONFIG_EKF2_WIND
-	//
 #if defined(CONFIG_EKF2_TERRAIN)
 	// assume a ground clearance
 	_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
@@ -101,6 +79,13 @@ void Ekf::reset()

 	_output_predictor.reset();

+	_time_latest_us = 0;
+	_time_delayed_us = 0;
+
+	_imu_updated = false;
+
+	_filter_initialised = false;
+
 	// Ekf private fields
 	_time_last_horizontal_aiding = 0;
 	_time_last_v_pos_aiding = 0;
@@ -133,18 +118,12 @@ void Ekf::reset()
 	_zero_velocity_update.reset();

 	updateParameters();
+
+	initialiseCovariance();
 }

 bool Ekf::update()
 {
-	if (!_filter_initialised) {
-		_filter_initialised = initialiseFilter();
-
-		if (!_filter_initialised) {
-			return false;
-		}
-	}
-
 	// Only run the filter if IMU data in the buffer has been updated
 	if (_imu_updated) {
 		_imu_updated = false;
@@ -159,14 +138,52 @@ bool Ekf::update()
 		float filter_update_s = 1e-6f * _params.filter_update_interval_us;
 		_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);

+		// Filter accel for tilt initialization
+		if (_is_first_imu_sample) {
+			_accel_lpf.reset(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt);
+			_gyro_lpf.reset(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt);
+			_is_first_imu_sample = false;
+
+			_state = {};
+			initialiseTilt(_accel_lpf.getState());
+			initialiseCovariance();
+
+		} else {
+			_accel_lpf.update(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt);
+			_gyro_lpf.update(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt);
+		}
+
+		if (!_filter_initialised) {
+
+			const float accel_norm = _accel_lpf.getState().norm();
+			const float gyro_norm = _gyro_lpf.getState().norm();
+
+			if ((accel_norm > 0.8f * CONSTANTS_ONE_G)
+			    && (accel_norm < 1.2f * CONSTANTS_ONE_G)
+			    && (gyro_norm < math::radians(15.f))
+			   ) {
+				// once ready reset and init tilt from filtered accel
+				_state = {};
+				initialiseTilt(_accel_lpf.getState());
+				initialiseCovariance();
+
+				// reset the output predictor state history to match the EKF initial values
+				_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos);
+
+				_filter_initialised = true;
+			}
+		}
+
 		updateIMUBiasInhibit(imu_sample_delayed);

 		// perform state and covariance prediction for the main filter
 		predictCovariance(imu_sample_delayed);
 		predictState(imu_sample_delayed);

-		// control fusion of observation data
-		controlFusionModes(imu_sample_delayed);
+		if (_filter_initialised) {
+			// control fusion of observation data
+			controlFusionModes(imu_sample_delayed);
+		}

 		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos,
 						      _state.gyro_bias, _state.accel_bias);
@@ -177,52 +194,10 @@ bool Ekf::update()
 	return false;
 }

-bool Ekf::initialiseFilter()
+bool Ekf::initialiseTilt(const Vector3f &accel)
 {
-	// Filter accel for tilt initialization
-	const imuSample &imu_init = _imu_buffer.get_newest();
-
-	// protect against zero data
-	if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
-		return false;
-	}
-
-	if (_is_first_imu_sample) {
-		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
-		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
-		_is_first_imu_sample = false;
-
-	} else {
-		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
-		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
-	}
-
-	if (!initialiseTilt()) {
-		return false;
-	}
-
-	// initialise the state covariance matrix now we have starting values for all the states
-	initialiseCovariance();
-
-	// reset the output predictor state history to match the EKF initial values
-	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos);
-
-	return true;
-}
-
-bool Ekf::initialiseTilt()
-{
-	const float accel_norm = _accel_lpf.getState().norm();
-	const float gyro_norm = _gyro_lpf.getState().norm();
-
-	if (accel_norm < 0.8f * CONSTANTS_ONE_G ||
-	    accel_norm > 1.2f * CONSTANTS_ONE_G ||
-	    gyro_norm > math::radians(15.0f)) {
-		return false;
-	}
-
 	// get initial tilt estimate from delta velocity vector, assuming vehicle is static
-	_state.quat_nominal = Quatf(_accel_lpf.getState(), Vector3f(0.f, 0.f, -1.f));
+	_state.quat_nominal = Quatf(accel, Vector3f(0.f, 0.f, -1.f));
 	_R_to_earth = Dcmf(_state.quat_nominal);

 	return true;
@@ -80,15 +80,15 @@ public:
 	Ekf()
 	{
 		reset();
+		initialise_interface();
 	};

-	virtual ~Ekf() = default;
-
-	// initialise variables to sane values (also interface class)
-	bool init(uint64_t timestamp) override;
+	~Ekf() = default;

 	void print_status();

+	void reset();
+
 	// should be called every time new data is pushed into the filter
 	bool update();

@@ -427,10 +427,7 @@ private:
 	friend class EvVelLocalFrameNed;
 	friend class EvVelLocalFrameFrd;

-	// set the internal states and status to their default value
-	void reset();
-
-	bool initialiseTilt();
+	bool initialiseTilt(const Vector3f &accel);

 	// check if the EKF is dead reckoning horizontal velocity using inertial data only
 	void updateDeadReckoningStatus();
@@ -583,11 +580,6 @@ private:
 	estimator_aid_source2d_s _aid_src_aux_vel {};
 #endif // CONFIG_EKF2_AUXVEL

-	// Variables used by the initial filter alignment
-	bool _is_first_imu_sample{true};
-	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
-	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	estimator_aid_source1d_s _aid_src_baro_hgt {};

@@ -626,9 +618,6 @@ private:
 	uint64_t _time_good_vert_accel{0};	///< last time a good vertical accel was detected (uSec)
 	uint16_t _clip_counter[3];		///< counter per axis that increments when clipping ad decrements when not

-	// initialise filter states of both the delayed ekf and the real time complementary filter
-	bool initialiseFilter(void);
-
 	// initialise ekf covariance matrix
 	void initialiseCovariance();

@@ -78,11 +78,6 @@ EstimatorInterface::~EstimatorInterface()
 // Accumulate imu data and store to buffer at desired rate
 void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 {
-	// TODO: resolve misplaced responsibility
-	if (!_initialised) {
-		_initialised = init(imu_sample.time_us);
-	}
-
 	_time_latest_us = imu_sample.time_us;

 	// the output observer always runs
@@ -122,18 +117,19 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 void EstimatorInterface::setMagData(const magSample &mag_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_mag_buffer == nullptr) {
-		_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);

-		if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
-			delete _mag_buffer;
-			_mag_buffer = nullptr;
-			printBufferAllocationFailed("mag");
+			if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
+				delete _mag_buffer;
+				_mag_buffer = nullptr;
+				printBufferAllocationFailed("mag");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -161,18 +157,19 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 #if defined(CONFIG_EKF2_GNSS)
 void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_gps_buffer == nullptr) {
-		_gps_buffer = new RingBuffer<gnssSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_gps_buffer = new RingBuffer<gnssSample>(_obs_buffer_length);

-		if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
-			delete _gps_buffer;
-			_gps_buffer = nullptr;
-			printBufferAllocationFailed("GPS");
+			if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
+				delete _gps_buffer;
+				_gps_buffer = nullptr;
+				printBufferAllocationFailed("GPS");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -208,18 +205,19 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 #if defined(CONFIG_EKF2_BAROMETER)
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_baro_buffer == nullptr) {
-		_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);

-		if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
-			delete _baro_buffer;
-			_baro_buffer = nullptr;
-			printBufferAllocationFailed("baro");
+			if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
+				delete _baro_buffer;
+				_baro_buffer = nullptr;
+				printBufferAllocationFailed("baro");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -247,18 +245,19 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 #if defined(CONFIG_EKF2_AIRSPEED)
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_airspeed_buffer == nullptr) {
-		_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);

-		if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
-			delete _airspeed_buffer;
-			_airspeed_buffer = nullptr;
-			printBufferAllocationFailed("airspeed");
+			if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
+				delete _airspeed_buffer;
+				_airspeed_buffer = nullptr;
+				printBufferAllocationFailed("airspeed");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -285,18 +284,19 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);

-		if (_range_buffer == nullptr || !_range_buffer->valid()) {
-			delete _range_buffer;
-			_range_buffer = nullptr;
-			printBufferAllocationFailed("range");
+			if (_range_buffer == nullptr || !_range_buffer->valid()) {
+				delete _range_buffer;
+				_range_buffer = nullptr;
+				printBufferAllocationFailed("range");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -324,18 +324,19 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_flow_buffer == nullptr) {
-		_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
+		if (_imu_buffer_length > 0) {
+			_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);

-		if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
-			delete _flow_buffer;
-			_flow_buffer = nullptr;
-			printBufferAllocationFailed("flow");
+			if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
+				delete _flow_buffer;
+				_flow_buffer = nullptr;
+				printBufferAllocationFailed("flow");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -362,18 +363,19 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_ext_vision_buffer == nullptr) {
-		_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);

-		if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
-			delete _ext_vision_buffer;
-			_ext_vision_buffer = nullptr;
-			printBufferAllocationFailed("vision");
+			if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
+				delete _ext_vision_buffer;
+				_ext_vision_buffer = nullptr;
+				printBufferAllocationFailed("vision");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -402,18 +404,19 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 #if defined(CONFIG_EKF2_AUXVEL)
 void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_auxvel_buffer == nullptr) {
-		_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);

-		if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
-			delete _auxvel_buffer;
-			_auxvel_buffer = nullptr;
-			printBufferAllocationFailed("aux vel");
+			if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
+				delete _auxvel_buffer;
+				_auxvel_buffer = nullptr;
+				printBufferAllocationFailed("aux vel");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -439,18 +442,19 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)

 void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_system_flag_buffer == nullptr) {
-		_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);

-		if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
-			delete _system_flag_buffer;
-			_system_flag_buffer = nullptr;
-			printBufferAllocationFailed("system flag");
+			if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
+				delete _system_flag_buffer;
+				_system_flag_buffer = nullptr;
+				printBufferAllocationFailed("system flag");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -477,16 +481,21 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
 	// sufficient samples have been collected
-	if (_params.drag_ctrl > 0) {
+	if (_params.drag_ctrl) {

 		// Allocate the required buffer size if not previously done
 		if (_drag_buffer == nullptr) {
-			_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
+			if (_obs_buffer_length > 0) {
+				_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);

-			if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
-				delete _drag_buffer;
-				_drag_buffer = nullptr;
-				printBufferAllocationFailed("drag");
+				if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
+					delete _drag_buffer;
+					_drag_buffer = nullptr;
+					printBufferAllocationFailed("drag");
+					return;
+				}
+
+			} else {
 				return;
 			}
 		}
@@ -536,7 +545,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 }
 #endif // CONFIG_EKF2_DRAG_FUSION

-bool EstimatorInterface::initialise_interface(uint64_t timestamp)
+bool EstimatorInterface::initialise_interface()
 {
 	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;

@@ -560,11 +569,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		return false;
 	}

-	_time_delayed_us = timestamp;
-	_time_latest_us = timestamp;
-
-	_fault_status.value = 0;
-
 	return true;
 }

@@ -318,9 +318,7 @@ public:
 protected:

 	EstimatorInterface() = default;
-	virtual ~EstimatorInterface();
-
-	virtual bool init(uint64_t timestamp) = 0;
+	~EstimatorInterface();

 	parameters _params{};		// filter parameters

@@ -346,6 +344,11 @@ protected:
 	uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon
 	uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data

+	// Variables used by the initial filter alignment
+	bool _is_first_imu_sample{true};
+	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
+	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+
 	OutputPredictor _output_predictor{};

 #if defined(CONFIG_EKF2_AIRSPEED)
@@ -378,7 +381,6 @@ protected:
 	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};		// air density (kg/m**3)

 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
-	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized

 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	MapProjection _local_origin_lat_lon{};
@@ -449,7 +451,7 @@ protected:
 	fault_status_u _fault_status{};

 	// allocate data buffers and initialize interface variables
-	bool initialise_interface(uint64_t timestamp);
+	bool initialise_interface();

 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control
@@ -337,9 +337,7 @@ def predict_sideslip(
    vel_rel = state["vel"] - wind
    relative_wind_body = state["quat_nominal"].inverse() * vel_rel

-    # Small angle approximation of side slip model
-    # Protect division by zero using epsilon
-    sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon)
+    sideslip_pred = sf.atan2(relative_wind_body[1], relative_wind_body[0], epsilon)

    return sideslip_pred

@@ -30,66 +30,65 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 513
+  // Total ops: 518

  // Input arrays

-  // Intermediate terms (43)
-  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
-  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp3 = 4 * _tmp2;
-  const Scalar _tmp4 = 2 * state(6, 0);
-  const Scalar _tmp5 = _tmp4 * state(0, 0);
-  const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp8 = 2 * state(0, 0);
-  const Scalar _tmp9 = _tmp8 * state(3, 0);
-  const Scalar _tmp10 = 2 * state(2, 0);
-  const Scalar _tmp11 = _tmp10 * state(1, 0);
-  const Scalar _tmp12 = _tmp11 + _tmp9;
-  const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0);
-  const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7;
-  const Scalar _tmp15 =
-      _tmp14 + epsilon * (2 * math::min<Scalar>(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1);
-  const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp17 = _tmp11 - _tmp9;
-  const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0);
-  const Scalar _tmp19 =
-      (_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2));
-  const Scalar _tmp20 = _tmp4 * state(3, 0);
-  const Scalar _tmp21 = Scalar(1.0) / (_tmp15);
-  const Scalar _tmp22 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20);
-  const Scalar _tmp23 = 4 * _tmp0;
-  const Scalar _tmp24 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5);
-  const Scalar _tmp25 = 2 * state(3, 0);
-  const Scalar _tmp26 = _tmp4 * state(2, 0);
-  const Scalar _tmp27 = _tmp4 * state(1, 0);
-  const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) +
-                        (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27);
-  const Scalar _tmp29 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26);
+  // Intermediate terms (50)
+  const Scalar _tmp0 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp1 = 2 * _tmp0;
+  const Scalar _tmp2 = 2 * state(3, 0);
+  const Scalar _tmp3 = _tmp2 * state(6, 0);
+  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp6 = _tmp2 * state(0, 0);
+  const Scalar _tmp7 = 2 * state(1, 0);
+  const Scalar _tmp8 = _tmp7 * state(2, 0);
+  const Scalar _tmp9 = _tmp6 + _tmp8;
+  const Scalar _tmp10 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp11 = 2 * state(2, 0);
+  const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0);
+  const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0);
+  const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
+  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
+  const Scalar _tmp16 = 2 * _tmp10;
+  const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0);
+  const Scalar _tmp18 = std::pow(_tmp14, Scalar(2));
+  const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp20 = -_tmp6 + _tmp8;
+  const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0);
+  const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0);
+  const Scalar _tmp23 = _tmp22 / _tmp18;
+  const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) -
+                        _tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17);
+  const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2)));
+  const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25;
+  const Scalar _tmp27 = _tmp26 * state(3, 0);
+  const Scalar _tmp28 = _tmp7 * state(6, 0);
+  const Scalar _tmp29 = _tmp11 * state(6, 0);
  const Scalar _tmp30 =
-      -_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0);
-  const Scalar _tmp31 =
-      _tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0);
-  const Scalar _tmp32 =
-      _tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0);
-  const Scalar _tmp33 = _tmp19 * _tmp7;
-  const Scalar _tmp34 = _tmp17 * _tmp21;
-  const Scalar _tmp35 = -_tmp33 + _tmp34;
-  const Scalar _tmp36 = _tmp12 * _tmp19;
-  const Scalar _tmp37 = _tmp16 * _tmp21;
-  const Scalar _tmp38 = -_tmp36 + _tmp37;
-  const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21;
-  const Scalar _tmp40 = _tmp33 - _tmp34;
-  const Scalar _tmp41 = _tmp36 - _tmp37;
-  const Scalar _tmp42 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
+      _tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29);
+  const Scalar _tmp31 = _tmp26 * state(1, 0);
+  const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) -
+                        _tmp23 * (_tmp16 * state(2, 0) + _tmp3);
+  const Scalar _tmp33 = _tmp26 * state(0, 0);
+  const Scalar _tmp34 = 4 * state(3, 0);
+  const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) -
+                        _tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28);
+  const Scalar _tmp36 = _tmp26 * state(2, 0);
+  const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36;
+  const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35;
+  const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35;
+  const Scalar _tmp40 = _tmp23 * _tmp5;
+  const Scalar _tmp41 = _tmp15 * _tmp20;
+  const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41);
+  const Scalar _tmp43 = _tmp15 * _tmp19;
+  const Scalar _tmp44 = _tmp23 * _tmp9;
+  const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44);
+  const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21);
+  const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41);
+  const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44);
+  const Scalar _tmp49 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));

  // Output terms (2)
  if (H != nullptr) {
@@ -97,91 +96,91 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,

    _h.setZero();

-    _h(0, 0) = _tmp30;
-    _h(1, 0) = _tmp31;
-    _h(2, 0) = _tmp32;
-    _h(3, 0) = _tmp35;
-    _h(4, 0) = _tmp38;
-    _h(5, 0) = _tmp39;
-    _h(21, 0) = _tmp40;
-    _h(22, 0) = _tmp41;
+    _h(0, 0) = _tmp37;
+    _h(1, 0) = _tmp38;
+    _h(2, 0) = _tmp39;
+    _h(3, 0) = _tmp42;
+    _h(4, 0) = _tmp45;
+    _h(5, 0) = _tmp46;
+    _h(21, 0) = _tmp47;
+    _h(22, 0) = _tmp48;
  }

  if (K != nullptr) {
    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

    _k(0, 0) =
-        _tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 +
-                  P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39);
+        _tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 +
+                  P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46);
    _k(1, 0) =
-        _tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 +
-                  P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39);
+        _tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 +
+                  P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46);
    _k(2, 0) =
-        _tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 +
-                  P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39);
+        _tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 +
+                  P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46);
    _k(3, 0) =
-        _tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 +
-                  P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39);
+        _tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 +
+                  P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46);
    _k(4, 0) =
-        _tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 +
-                  P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39);
+        _tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 +
+                  P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46);
    _k(5, 0) =
-        _tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 +
-                  P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39);
+        _tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 +
+                  P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46);
    _k(6, 0) =
-        _tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 +
-                  P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39);
+        _tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 +
+                  P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46);
    _k(7, 0) =
-        _tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 +
-                  P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39);
+        _tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 +
+                  P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46);
    _k(8, 0) =
-        _tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 +
-                  P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39);
+        _tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 +
+                  P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46);
    _k(9, 0) =
-        _tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 +
-                  P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39);
+        _tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 +
+                  P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46);
    _k(10, 0) =
-        _tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 +
-                  P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39);
+        _tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 +
+                  P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46);
    _k(11, 0) =
-        _tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 +
-                  P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39);
+        _tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 +
+                  P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46);
    _k(12, 0) =
-        _tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 +
-                  P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39);
+        _tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 +
+                  P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46);
    _k(13, 0) =
-        _tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 +
-                  P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39);
+        _tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 +
+                  P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46);
    _k(14, 0) =
-        _tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 +
-                  P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39);
+        _tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 +
+                  P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46);
    _k(15, 0) =
-        _tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 +
-                  P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39);
+        _tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 +
+                  P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46);
    _k(16, 0) =
-        _tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 +
-                  P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39);
+        _tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 +
+                  P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46);
    _k(17, 0) =
-        _tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 +
-                  P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39);
+        _tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 +
+                  P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46);
    _k(18, 0) =
-        _tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 +
-                  P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39);
+        _tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 +
+                  P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46);
    _k(19, 0) =
-        _tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 +
-                  P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39);
+        _tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 +
+                  P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46);
    _k(20, 0) =
-        _tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 +
-                  P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39);
+        _tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 +
+                  P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46);
    _k(21, 0) =
-        _tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 +
-                  P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39);
+        _tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 +
+                  P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46);
    _k(22, 0) =
-        _tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 +
-                  P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39);
+        _tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 +
+                  P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46);
    _k(23, 0) =
-        _tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 +
-                  P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39);
+        _tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 +
+                  P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46);
  }
 }  // NOLINT(readability/fn_size)

@@ -30,70 +30,69 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                     const Scalar epsilon, Scalar* const innov = nullptr,
                                     Scalar* const innov_var = nullptr) {
-  // Total ops: 265
+  // Total ops: 266

  // Input arrays

-  // Intermediate terms (42)
+  // Intermediate terms (49)
  const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp3 = 2 * state(0, 0);
-  const Scalar _tmp4 = _tmp3 * state(3, 0);
+  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp2 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp3 = 2 * state(3, 0);
+  const Scalar _tmp4 = _tmp3 * state(0, 0);
  const Scalar _tmp5 = 2 * state(1, 0);
  const Scalar _tmp6 = _tmp5 * state(2, 0);
-  const Scalar _tmp7 = _tmp4 + _tmp6;
-  const Scalar _tmp8 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp7 = -_tmp4 + _tmp6;
+  const Scalar _tmp8 = -state(22, 0) + state(4, 0);
  const Scalar _tmp9 = 2 * state(2, 0);
-  const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp10 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
  const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
-  const Scalar _tmp12 =
-      _tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
-  const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
-  const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp15 = -_tmp4 + _tmp6;
-  const Scalar _tmp16 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
-  const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
-  const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
-  const Scalar _tmp19 = _tmp18 * _tmp7;
-  const Scalar _tmp20 = _tmp13 * _tmp14;
-  const Scalar _tmp21 = _tmp19 - _tmp20;
-  const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
-  const Scalar _tmp23 = _tmp1 * _tmp18;
-  const Scalar _tmp24 = _tmp13 * _tmp15;
-  const Scalar _tmp25 = -_tmp23 + _tmp24;
-  const Scalar _tmp26 = 2 * state(6, 0);
-  const Scalar _tmp27 = _tmp26 * state(0, 0);
-  const Scalar _tmp28 = _tmp26 * state(3, 0);
-  const Scalar _tmp29 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) -
-      Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8);
-  const Scalar _tmp30 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) -
-      Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9);
-  const Scalar _tmp31 = 2 * state(3, 0);
-  const Scalar _tmp32 = _tmp26 * state(2, 0);
-  const Scalar _tmp33 = _tmp26 * state(1, 0);
-  const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) -
-                        Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32);
-  const Scalar _tmp35 = 4 * state(3, 0);
-  const Scalar _tmp36 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) -
-      Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33);
-  const Scalar _tmp37 =
-      _tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0);
-  const Scalar _tmp38 =
-      -_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0);
-  const Scalar _tmp39 =
-      _tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0);
-  const Scalar _tmp40 = _tmp23 - _tmp24;
-  const Scalar _tmp41 = -_tmp19 + _tmp20;
+  const Scalar _tmp12 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp13 = _tmp4 + _tmp6;
+  const Scalar _tmp14 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp15 = _tmp12 * _tmp8 + _tmp13 * _tmp2 + _tmp14 * state(6, 0);
+  const Scalar _tmp16 = _tmp15 + epsilon * ((((_tmp15) > 0) - ((_tmp15) < 0)) + Scalar(0.5));
+  const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
+  const Scalar _tmp18 = _tmp1 * _tmp17;
+  const Scalar _tmp19 = std::pow(_tmp16, Scalar(2));
+  const Scalar _tmp20 = _tmp11 / _tmp19;
+  const Scalar _tmp21 = _tmp13 * _tmp20;
+  const Scalar _tmp22 = _tmp19 / (std::pow(_tmp11, Scalar(2)) + _tmp19);
+  const Scalar _tmp23 = _tmp22 * (-_tmp18 + _tmp21);
+  const Scalar _tmp24 = _tmp12 * _tmp20;
+  const Scalar _tmp25 = _tmp17 * _tmp7;
+  const Scalar _tmp26 = _tmp22 * (-_tmp24 + _tmp25);
+  const Scalar _tmp27 = _tmp22 * (_tmp10 * _tmp17 - _tmp14 * _tmp20);
+  const Scalar _tmp28 = _tmp3 * state(6, 0);
+  const Scalar _tmp29 = 4 * _tmp8;
+  const Scalar _tmp30 = 2 * state(0, 0);
+  const Scalar _tmp31 = _tmp30 * state(6, 0);
+  const Scalar _tmp32 =
+      _tmp17 * (_tmp28 + _tmp5 * _tmp8) - _tmp20 * (_tmp2 * _tmp5 - _tmp29 * state(2, 0) - _tmp31);
+  const Scalar _tmp33 = (Scalar(1) / Scalar(2)) * _tmp22;
+  const Scalar _tmp34 = _tmp33 * state(1, 0);
+  const Scalar _tmp35 = _tmp5 * state(6, 0);
+  const Scalar _tmp36 = _tmp9 * state(6, 0);
+  const Scalar _tmp37 = _tmp17 * (-_tmp3 * _tmp8 + _tmp35) - _tmp20 * (_tmp2 * _tmp3 - _tmp36);
+  const Scalar _tmp38 = _tmp33 * state(3, 0);
+  const Scalar _tmp39 = 4 * _tmp2;
+  const Scalar _tmp40 =
+      _tmp17 * (_tmp31 - _tmp39 * state(1, 0) + _tmp8 * _tmp9) - _tmp20 * (_tmp2 * _tmp9 + _tmp28);
+  const Scalar _tmp41 = _tmp33 * state(2, 0);
+  const Scalar _tmp42 = _tmp17 * (-_tmp30 * _tmp8 + _tmp36 - _tmp39 * state(3, 0)) -
+                        _tmp20 * (_tmp2 * _tmp30 - _tmp29 * state(3, 0) + _tmp35);
+  const Scalar _tmp43 = _tmp33 * state(0, 0);
+  const Scalar _tmp44 = _tmp32 * _tmp34 - _tmp37 * _tmp38 - _tmp40 * _tmp41 + _tmp42 * _tmp43;
+  const Scalar _tmp45 = -_tmp32 * _tmp38 - _tmp34 * _tmp37 + _tmp40 * _tmp43 + _tmp41 * _tmp42;
+  const Scalar _tmp46 = _tmp32 * _tmp43 - _tmp34 * _tmp42 - _tmp37 * _tmp41 + _tmp38 * _tmp40;
+  const Scalar _tmp47 = _tmp22 * (_tmp24 - _tmp25);
+  const Scalar _tmp48 = _tmp22 * (_tmp18 - _tmp21);

  // Output terms (2)
  if (innov != nullptr) {
    Scalar& _innov = (*innov);

-    _innov = _tmp13 * _tmp17;
+    _innov = std::atan2(_tmp11, _tmp16);
  }

  if (innov_var != nullptr) {
@@ -101,22 +100,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,

    _innov_var =
        R +
-        _tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 +
-                  P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) +
-        _tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 +
-                  P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) +
-        _tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 +
-                  P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) +
-        _tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 +
-                  P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) +
-        _tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 +
-                  P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) +
-        _tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 +
-                  P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) +
-        _tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 +
-                  P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) +
-        _tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 +
-                  P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22);
+        _tmp23 * (P(0, 22) * _tmp45 + P(1, 22) * _tmp46 + P(2, 22) * _tmp44 + P(21, 22) * _tmp47 +
+                  P(22, 22) * _tmp23 + P(3, 22) * _tmp26 + P(4, 22) * _tmp48 + P(5, 22) * _tmp27) +
+        _tmp26 * (P(0, 3) * _tmp45 + P(1, 3) * _tmp46 + P(2, 3) * _tmp44 + P(21, 3) * _tmp47 +
+                  P(22, 3) * _tmp23 + P(3, 3) * _tmp26 + P(4, 3) * _tmp48 + P(5, 3) * _tmp27) +
+        _tmp27 * (P(0, 5) * _tmp45 + P(1, 5) * _tmp46 + P(2, 5) * _tmp44 + P(21, 5) * _tmp47 +
+                  P(22, 5) * _tmp23 + P(3, 5) * _tmp26 + P(4, 5) * _tmp48 + P(5, 5) * _tmp27) +
+        _tmp44 * (P(0, 2) * _tmp45 + P(1, 2) * _tmp46 + P(2, 2) * _tmp44 + P(21, 2) * _tmp47 +
+                  P(22, 2) * _tmp23 + P(3, 2) * _tmp26 + P(4, 2) * _tmp48 + P(5, 2) * _tmp27) +
+        _tmp45 * (P(0, 0) * _tmp45 + P(1, 0) * _tmp46 + P(2, 0) * _tmp44 + P(21, 0) * _tmp47 +
+                  P(22, 0) * _tmp23 + P(3, 0) * _tmp26 + P(4, 0) * _tmp48 + P(5, 0) * _tmp27) +
+        _tmp46 * (P(0, 1) * _tmp45 + P(1, 1) * _tmp46 + P(2, 1) * _tmp44 + P(21, 1) * _tmp47 +
+                  P(22, 1) * _tmp23 + P(3, 1) * _tmp26 + P(4, 1) * _tmp48 + P(5, 1) * _tmp27) +
+        _tmp47 * (P(0, 21) * _tmp45 + P(1, 21) * _tmp46 + P(2, 21) * _tmp44 + P(21, 21) * _tmp47 +
+                  P(22, 21) * _tmp23 + P(3, 21) * _tmp26 + P(4, 21) * _tmp48 + P(5, 21) * _tmp27) +
+        _tmp48 * (P(0, 4) * _tmp45 + P(1, 4) * _tmp46 + P(2, 4) * _tmp44 + P(21, 4) * _tmp47 +
+                  P(22, 4) * _tmp23 + P(3, 4) * _tmp26 + P(4, 4) * _tmp48 + P(5, 4) * _tmp27);
  }
 }  // NOLINT(readability/fn_size)

@@ -833,6 +833,14 @@ void EKF2::Run()
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 			UpdateMagCalibration(now);
 #endif // CONFIG_EKF2_MAGNETOMETER
+
+		} else {
+			// if filter not initialized
+
+
+
+
+
 		}

 		// publish ekf2_timestamps
@@ -1,6 +1,6 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
-90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
+10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
+90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
 190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,0,0,-1.2e+02,-3e-11,-2.6e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082
 290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,0,0,-1.2e+02,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11
 390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0,0,-1.2e+02,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13
@@ -1,6 +1,6 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
-90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
+10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
+90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
 190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.035,0,0,-4.9e+02,1.6e-09,-1.4e-09,-6.4e-11,0,0,-3.2e-06,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082
 290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.038,0,0,-4.9e+02,7.3e-09,-1.1e-08,-3.9e-10,0,0,-2e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11
 390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.05,0,0,-4.9e+02,-2.1e-10,-1.4e-08,-3.2e-10,0,0,-1.4e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13
@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -54,7 +54,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_is_fixed_wing(false);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}
@@ -56,7 +56,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -65,7 +65,7 @@ public:
 		_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);

 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -60,7 +60,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}
@@ -58,7 +58,7 @@ public:
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf_wrapper.disableBaroHeightFusion();
 		_ekf_wrapper.disableRangeHeightFusion();
 		_sensor_simulator.runSeconds(0.1);
@@ -47,8 +47,7 @@ public:
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
-		_ekf.init(0);
-
+		_ekf.reset();
 	}

 	void TearDown() override
@@ -56,7 +56,7 @@ public:
 	void SetUp() override
 	{
 		// first init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}
@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}
@@ -45,7 +45,7 @@ public:

 	void SetUp() override
 	{
-		_ekf->init(0);
+		_ekf->reset();
 	}
 };

@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
@@ -35,5 +35,5 @@ px4_add_library(RoverAckermannControl
 	RoverAckermannControl.cpp
 )

-target_link_libraries(RoverAckermannControl PUBLIC pid)
+target_link_libraries(RoverAckermannControl PUBLIC PID)
 target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -41,27 +41,19 @@ RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParam
 {
 	updateParams();
 	_rover_ackermann_status_pub.advertise();
-	pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
-
 }

 void RoverAckermannControl::updateParams()
 {
 	ModuleParams::updateParams();

-	pid_set_parameters(&_pid_throttle,
-			   _param_ra_speed_p.get(), // Proportional gain
-			   _param_ra_speed_i.get(), // Integral gain
-			   0, // Derivative gain
-			   _param_ra_speed_i.get() > FLT_EPSILON ? 1.f / _param_ra_speed_i.get() : 0.f, // Integral limit
-			   1); // Output limit
+	_pid_throttle.setGains(_param_ra_speed_p.get(), _param_ra_speed_i.get(), 0.f);
+	_pid_throttle.setIntegralLimit(1.f);
+	_pid_throttle.setOutputLimit(1.f);

-	pid_set_parameters(&_pid_lat_accel,
-			   _param_ra_lat_accel_p.get(), // Proportional gain
-			   _param_ra_lat_accel_i.get(), // Integral gain
-			   0, // Derivative gain
-			   _param_ra_lat_accel_i.get() > FLT_EPSILON ? 1.f / _param_ra_lat_accel_i.get() : 0.f, // Integral limit
-			   1); // Output limit
+	_pid_lat_accel.setGains(_param_ra_lat_accel_p.get(), _param_ra_lat_accel_i.get(), 0.f);
+	_pid_lat_accel.setIntegralLimit(1.f);
+	_pid_lat_accel.setOutputLimit(1.f);

 	// Update slew rates
 	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
@@ -117,8 +109,8 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe

 			if (sign(vehicle_forward_speed_temp) ==
 			    1) { // Only do closed loop control when driving forwards (backwards driving is non-minimum phase and can therefor introduce instability)
-				steering_setpoint += pid_calculate(&_pid_lat_accel, _rover_ackermann_setpoint.lateral_acceleration_setpoint,
-								   vehicle_lateral_acceleration, 0, dt);
+				_pid_lat_accel.setSetpoint(_rover_ackermann_setpoint.lateral_acceleration_setpoint);
+				steering_setpoint += _pid_lat_accel.update(vehicle_lateral_acceleration, dt);
 			}

 			_rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(),
@@ -156,8 +148,8 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe
 	_rover_ackermann_status.steering_setpoint_normalized = steering_normalized;
 	_rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState();
 	_rover_ackermann_status.measured_lateral_acceleration = vehicle_lateral_acceleration;
-	_rover_ackermann_status.pid_throttle_integral = _pid_throttle.integral *  _param_ra_speed_i.get();
-	_rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.integral * _param_ra_lat_accel_i.get();
+	_rover_ackermann_status.pid_throttle_integral = _pid_throttle.getIntegral();
+	_rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.getIntegral();
 	_rover_ackermann_status_pub.publish(_rover_ackermann_status);

 	// Publish to motor
@@ -218,8 +210,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe
 						   -1.f, 1.f);
 		}

-		forward_speed_normalized += pid_calculate(&_pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
-					    vehicle_forward_speed, 0, dt); // Feedback
+		_pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState());
+		forward_speed_normalized += _pid_throttle.update(vehicle_forward_speed, dt);
 	}

 	return math::constrain(forward_speed_normalized, -1.f, 1.f);
@@ -228,8 +220,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe

 void RoverAckermannControl::resetControllers()
 {
-	pid_reset_integral(&_pid_throttle);
-	pid_reset_integral(&_pid_lat_accel);
+	_pid_throttle.resetIntegral();
+	_pid_lat_accel.resetIntegral();
 	_forward_speed_setpoint_with_accel_limit.setForcedValue(0.f);
 	_steering_with_rate_limit.setForcedValue(0.f);

@@ -46,7 +46,7 @@
 #include <uORB/topics/actuator_servos.h>

 // Standard libraries
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #include <matrix/matrix/math.hpp>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -114,8 +114,8 @@ private:
 	hrt_abstime _timestamp{0};

 	// Controllers
-	PID_t _pid_throttle; // The PID controller for the closed loop speed control
-	PID_t _pid_lat_accel; // The PID controller for the closed loop speed control
+	PID _pid_throttle; // The PID controller for the closed loop speed control
+	PID _pid_lat_accel; // The PID controller for the closed loop speed control
 	SlewRate<float> _steering_with_rate_limit{0.f};
 	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};

@@ -57,7 +57,7 @@
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <math.h>
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>

 using namespace matrix;

@@ -35,5 +35,5 @@ px4_add_library(RoverDifferentialControl
 	RoverDifferentialControl.cpp
 )

-target_link_libraries(RoverDifferentialControl PUBLIC pid)
+target_link_libraries(RoverDifferentialControl PUBLIC PID)
 target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -42,9 +42,6 @@ RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : Modul
 {
 	updateParams();
 	_rover_differential_status_pub.advertise();
-	pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
-	pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
-	pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f);
 }

 void RoverDifferentialControl::updateParams()
@@ -54,24 +51,15 @@ void RoverDifferentialControl::updateParams()
 	_max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F;

 	// Update PID
-	pid_set_parameters(&_pid_yaw_rate,
-			   _param_rd_yaw_rate_p.get(), // Proportional gain
-			   _param_rd_yaw_rate_i.get(), // Integral gain
-			   0.f, // Derivative gain
-			   1.f, // Integral limit
-			   1.f); // Output limit
-	pid_set_parameters(&_pid_throttle,
-			   _param_rd_p_gain_speed.get(), // Proportional gain
-			   _param_rd_i_gain_speed.get(), // Integral gain
-			   0.f, // Derivative gain
-			   1.f, // Integral limit
-			   1.f); // Output limit
-	pid_set_parameters(&_pid_yaw,
-			   _param_rd_p_gain_yaw.get(),  // Proportional gain
-			   _param_rd_i_gain_yaw.get(),  // Integral gain
-			   0.f,  // Derivative gain
-			   _max_yaw_rate,  // Integral limit
-			   _max_yaw_rate);  // Output limit
+	_pid_yaw_rate.setGains(_param_rd_yaw_rate_p.get(), _param_rd_yaw_rate_i.get(), 0.f);
+	_pid_yaw_rate.setIntegralLimit(1.f);
+	_pid_yaw_rate.setOutputLimit(1.f);
+	_pid_throttle.setGains(_param_rd_p_gain_speed.get(), _param_rd_i_gain_speed.get(), 0.f);
+	_pid_throttle.setIntegralLimit(1.f);
+	_pid_throttle.setOutputLimit(1.f);
+	_pid_yaw.setGains(_param_rd_p_gain_yaw.get(), _param_rd_i_gain_yaw.get(), 0.f);
+	_pid_yaw.setIntegralLimit(_max_yaw_rate);
+	_pid_yaw.setOutputLimit(_max_yaw_rate);

 	// Update slew rates
 	if (_max_yaw_rate > FLT_EPSILON) {
@@ -99,8 +87,10 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
 		_yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt);
 		_rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState());
-		const float heading_error = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - vehicle_yaw);
-		_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
+		_pid_yaw.setSetpoint(
+			matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() -
+					vehicle_yaw));  // error as setpoint to take care of wrapping
+		_rover_differential_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt);
 		_rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint;

 	} else {
@@ -142,9 +132,9 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	_rover_differential_status.measured_forward_speed = vehicle_forward_speed;
 	_rover_differential_status.measured_yaw = vehicle_yaw;
 	_rover_differential_status.measured_yaw_rate = vehicle_yaw_rate;
-	_rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
-	_rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
-	_rover_differential_status.pid_yaw_integral = _pid_yaw.integral;
+	_rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
+	_rover_differential_status.pid_throttle_integral = _pid_throttle.getIntegral();
+	_rover_differential_status.pid_yaw_integral = _pid_yaw.getIntegral();
 	_rover_differential_status_pub.publish(_rover_differential_status);

 	// Publish to motors
@@ -158,7 +148,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate,
 		const float max_thr_yaw_r,
 		const float max_yaw_accel, const float wheel_track, const float dt, SlewRate<float> &yaw_rate_with_accel_limit,
-		PID_t &pid_yaw_rate, const bool normalized)
+		PID &pid_yaw_rate, const bool normalized)
 {
 	float slew_rate_normalization{1.f};

@@ -190,8 +180,8 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set
 						max_thr_yaw_r, -1.f, 1.f);
 		}

-		speed_diff_normalized += pid_calculate(&pid_yaw_rate, yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0,
-						       dt); // Feedback
+		pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState());
+		speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt);
 	}

 	return math::constrain(speed_diff_normalized, -1.f, 1.f);
@@ -200,7 +190,7 @@ float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_set

 float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint,
 		const float vehicle_forward_speed, const float max_thr_spd, SlewRate<float> &forward_speed_setpoint_with_accel_limit,
-		PID_t &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized)
+		PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized)
 {
 	float slew_rate_normalization{1.f};

@@ -242,8 +232,8 @@ float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_
 						   -1.f, 1.f);
 		}

-		forward_speed_normalized += pid_calculate(&pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
-					    vehicle_forward_speed, 0, dt); // Feedback
+		pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState());
+		forward_speed_normalized += pid_throttle.update(vehicle_forward_speed, dt);
 	}

 	return math::constrain(forward_speed_normalized, -1.f, 1.f);
@@ -267,7 +257,7 @@ matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forwar

 void RoverDifferentialControl::resetControllers()
 {
-	pid_reset_integral(&_pid_throttle);
-	pid_reset_integral(&_pid_yaw_rate);
-	pid_reset_integral(&_pid_yaw);
+	_pid_throttle.resetIntegral();
+	_pid_yaw_rate.resetIntegral();
+	_pid_yaw.resetIntegral();
 }
@@ -47,7 +47,7 @@
 #include <lib/slew_rate/SlewRateYaw.hpp>

 // Standard libraries
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #include <matrix/matrix/math.hpp>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -100,7 +100,7 @@ private:
 	 * @return Normalized speed differece setpoint with applied slew rates [-1, 1].
 	 */
 	float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel,
-				      float wheel_track, float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID_t &pid_yaw_rate, bool normalized);
+				      float wheel_track, float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized);
 	/**
 	 * @brief Compute normalized forward speed setpoint and apply slew rates.
 	 * to the forward speed setpoint and doing closed loop speed control if enabled.
@@ -116,7 +116,7 @@ private:
 	 * @return Normalized forward speed setpoint with applied slew rates [-1, 1].
 	 */
 	float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd,
-					  SlewRate<float> &forward_speed_setpoint_with_accel_limit, PID_t &pid_throttle, float max_accel, float max_decel,
+					  SlewRate<float> &forward_speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel,
 					  float dt, bool normalized);

 	/**
@@ -142,9 +142,9 @@ private:
 	float _max_yaw_accel{0.f};

 	// Controllers
-	PID_t _pid_throttle; // Closed loop speed control
-	PID_t _pid_yaw;      // Closed loop yaw control
-	PID_t _pid_yaw_rate; // Closed loop yaw rate control
+	PID _pid_throttle; // Closed loop speed control
+	PID _pid_yaw;      // Closed loop yaw control
+	PID _pid_yaw_rate; // Closed loop yaw rate control
 	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
 	SlewRate<float> _yaw_rate_with_accel_limit{0.f};
 	SlewRateYaw<float> _yaw_setpoint_with_yaw_rate_limit;
@@ -53,7 +53,7 @@
 #include <uORB/topics/rover_mecanum_setpoint.h>

 // Standard libraries
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #include <matrix/matrix/math.hpp>

 // Local includes
@@ -35,5 +35,5 @@ px4_add_library(RoverMecanumControl
 	RoverMecanumControl.cpp
 )

-target_link_libraries(RoverMecanumControl PUBLIC pid)
+target_link_libraries(RoverMecanumControl PUBLIC PID)
 target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -42,40 +42,28 @@ RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(pa
 {
 	updateParams();
 	_rover_mecanum_status_pub.advertise();
-	pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
-	pid_init(&_pid_forward_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
-	pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
-	pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f);
 }

 void RoverMecanumControl::updateParams()
 {
 	ModuleParams::updateParams();
+
+	_pid_yaw_rate.setGains(_param_rm_yaw_rate_p.get(), _param_rm_yaw_rate_i.get(), 0.f);
+	_pid_yaw_rate.setIntegralLimit(1.f);
+	_pid_yaw_rate.setOutputLimit(1.f);
+
+	_pid_forward_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f);
+	_pid_forward_throttle.setIntegralLimit(1.f);
+	_pid_forward_throttle.setOutputLimit(1.f);
+
+	_pid_lateral_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f);
+	_pid_lateral_throttle.setIntegralLimit(1.f);
+	_pid_lateral_throttle.setOutputLimit(1.f);
+
 	_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
-	pid_set_parameters(&_pid_yaw_rate,
-			   _param_rm_yaw_rate_p.get(), // Proportional gain
-			   _param_rm_yaw_rate_i.get(), // Integral gain
-			   0.f, // Derivative gain
-			   1.f, // Integral limit
-			   1.f); // Output limit
-	pid_set_parameters(&_pid_forward_throttle,
-			   _param_rm_p_gain_speed.get(), // Proportional gain
-			   _param_rm_i_gain_speed.get(), // Integral gain
-			   0.f, // Derivative gain
-			   1.f, // Integral limit
-			   1.f); // Output limit
-	pid_set_parameters(&_pid_lateral_throttle,
-			   _param_rm_p_gain_speed.get(), // Proportional gain
-			   _param_rm_i_gain_speed.get(), // Integral gain
-			   0.f, // Derivative gain
-			   1.f, // Integral limit
-			   1.f); // Output limit
-	pid_set_parameters(&_pid_yaw,
-			   _param_rm_p_gain_yaw.get(),  // Proportional gain
-			   _param_rm_i_gain_yaw.get(),  // Integral gain
-			   0.f,  // Derivative gain
-			   _max_yaw_rate,  // Integral limit
-			   _max_yaw_rate);  // Output limit
+	_pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f);
+	_pid_yaw.setIntegralLimit(_max_yaw_rate);
+	_pid_yaw.setOutputLimit(_max_yaw_rate);
 }

 void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
@@ -91,11 +79,12 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl

 	// Closed loop yaw control
 	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) {
-		const float heading_error = matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw);
-		_rover_mecanum_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0.f, 0.f, dt);
+		_pid_yaw.setSetpoint(
+			matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw));  // error as setpoint to take care of wrapping
+		_rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt);

 	} else {
-		pid_reset_integral(&_pid_yaw);
+		_pid_yaw.resetIntegral();
 	}

 	// Yaw rate control
@@ -108,8 +97,9 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 						_param_rm_max_thr_yaw_r.get(), -1.f, 1.f);
 		}

+		_pid_yaw_rate.setSetpoint(_rover_mecanum_setpoint.yaw_rate_setpoint);
 		speed_diff_normalized = math::constrain(speed_diff_normalized +
-							pid_calculate(&_pid_yaw_rate, _rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0.f, dt),
+							_pid_yaw_rate.update(vehicle_yaw_rate, dt),
 							-1.f, 1.f); // Feedback

 	} else { // Use normalized setpoint
@@ -129,8 +119,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 					   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
 		}

-		forward_throttle += pid_calculate(&_pid_forward_throttle, _rover_mecanum_setpoint.forward_speed_setpoint,
-						  vehicle_forward_speed, 0, dt); // Feedback
+		_pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint);
+		forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt);

 		// Closed loop lateral speed control
 		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
@@ -138,8 +128,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 					   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
 		}

-		lateral_throttle += pid_calculate(&_pid_lateral_throttle, _rover_mecanum_setpoint.lateral_speed_setpoint,
-						  vehicle_lateral_speed, 0, dt); // Feedback
+		_pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint);
+		lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt);

 	} else { // Use normalized setpoint
 		forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain(
@@ -156,10 +146,10 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 	rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint;
 	rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate;
 	rover_mecanum_status.measured_yaw = vehicle_yaw;
-	rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
-	rover_mecanum_status.pid_yaw_integral = _pid_yaw.integral;
-	rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.integral;
-	rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.integral;
+	rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
+	rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral();
+	rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral();
+	rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral();
 	_rover_mecanum_status_pub.publish(rover_mecanum_status);

 	// Publish to motors
@@ -213,8 +203,8 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr

 void RoverMecanumControl::resetControllers()
 {
-	pid_reset_integral(&_pid_forward_throttle);
-	pid_reset_integral(&_pid_lateral_throttle);
-	pid_reset_integral(&_pid_yaw_rate);
-	pid_reset_integral(&_pid_yaw);
+	_pid_forward_throttle.resetIntegral();
+	_pid_lateral_throttle.resetIntegral();
+	_pid_yaw_rate.resetIntegral();
+	_pid_yaw.resetIntegral();
 }
@@ -46,7 +46,7 @@


 // Standard libraries
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #include <matrix/matrix/math.hpp>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -112,10 +112,10 @@ private:
 	float _max_yaw_rate{0.f};

 	// Controllers
-	PID_t _pid_forward_throttle; // PID for the closed loop forward speed control
-	PID_t _pid_lateral_throttle; // PID for the closed loop lateral speed control
-	PID_t _pid_yaw; // PID for the closed loop yaw control
-	PID_t _pid_yaw_rate; // PID for the closed loop yaw rate control
+	PID _pid_forward_throttle; // PID for the closed loop forward speed control
+	PID _pid_lateral_throttle; // PID for the closed loop lateral speed control
+	PID _pid_yaw; // PID for the closed loop yaw control
+	PID _pid_yaw_rate; // PID for the closed loop yaw rate control

 	// Parameters
 	DEFINE_PARAMETERS(
@@ -38,5 +38,5 @@ px4_add_module(
 		RoverPositionControl.hpp
 	DEPENDS
 		l1
-		pid
+		PID
 	)
@@ -92,13 +92,9 @@ void RoverPositionControl::parameters_update(bool force)
 		_gnd_control.set_l1_damping(_param_l1_damping.get());
 		_gnd_control.set_l1_period(_param_l1_period.get());

-		pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f);
-		pid_set_parameters(&_speed_ctrl,
-				   _param_speed_p.get(),
-				   _param_speed_i.get(),
-				   _param_speed_d.get(),
-				   _param_speed_imax.get(),
-				   _param_gndspeed_max.get());
+		_speed_ctrl.setGains(_param_speed_p.get(), _param_speed_i.get(), _param_speed_d.get());
+		_speed_ctrl.setIntegralLimit(_param_speed_imax.get());
+		_speed_ctrl.setOutputLimit(_param_gndspeed_max.get());
 	}
 }

@@ -239,12 +235,9 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 			const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 			const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));

-			const float x_vel = vel(0);
-			const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
-
 			// Compute airspeed control out and just scale it as a constant
-			mission_throttle = _param_throttle_speed_scaler.get()
-					   * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
+			_speed_ctrl.setSetpoint(mission_target_speed);
+			mission_throttle = _param_throttle_speed_scaler.get() * _speed_ctrl.update(vel(0), dt);

 			// Constrain throttle between min and max
 			mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get());
@@ -327,10 +320,8 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
 		const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 		const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2));

-		const float x_vel = vel(0);
-		const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
-
-		const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt);
+		_speed_ctrl.setSetpoint(desired_speed);
+		const float control_throttle = _speed_ctrl.update(vel(0), dt);

 		//Constrain maximum throttle to mission throttle
 		_throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle);
@@ -392,8 +383,6 @@ RoverPositionControl::Run()
 		vehicle_attitude_poll();
 		manual_control_setpoint_poll();

-		_vehicle_acceleration_sub.update();
-
 		/* update parameters from storage */
 		parameters_update();

@@ -48,7 +48,7 @@
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
@@ -128,8 +128,6 @@ private:
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};

-	uORB::SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
-
 	perf_counter_t	_loop_perf;			/**< loop performance counter */

 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
@@ -139,7 +137,7 @@ private:

 	/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
 	 the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
-	PID_t _speed_ctrl{};
+	PID _speed_ctrl{};

 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};		// captures the number of times the estimator has reset the horizontal position
@@ -327,9 +327,9 @@ void Sih::generate_fw_aerodynamics()
 {
 	_v_B = _C_IB.transpose() * _v_I; 	// velocity in body frame [m/s]
 	float altitude = _H0 - _p_I(2);
-	_wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX);
-	_wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
-	_tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]);
+	_wing_l.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
+	_wing_r.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX);
+	_tailplane.update_aero(_v_B, _w_B, altitude, -_u[1]*FLAP_MAX, _T_MAX * _u[3]);
 	_fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]);
 	_fuselage.update_aero(_v_B, _w_B, altitude);

@@ -409,11 +409,9 @@ void Sih::equations_of_motion(const float dt)
 			// integration: Euler forward
 			_p_I = _p_I + _p_I_dot * dt;
 			_v_I = _v_I + _v_I_dot * dt;
-			Eulerf RPY = Eulerf(_q);
-			RPY(0) = 0.0f;	// no roll
-			RPY(1) = radians(0.0f); // pitch slightly up if needed to get some lift
-			_q = Quatf(RPY);
-			_w_B.setZero();
+			_q = _q * _dq;
+			_q.normalize();
+			_w_B = constrain(_w_B + _w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F);
 			_grounded = true;
 		}

@@ -450,8 +448,7 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
 	// TODO: send differential pressure instead?
 	airspeed_s airspeed{};
 	airspeed.timestamp_sample = time_now_us;
-	// airspeed sensor is mounted along the negative Z axis since the vehicle is a tailsitter
-	airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f);
+	airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B.norm() + generate_wgn() * 0.2f);
 	airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
 	airspeed.air_temperature_celsius = NAN;
 	airspeed.confidence = 0.7f;
@@ -49,7 +49,6 @@
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/pid/pid.h>
 #include <matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -48,7 +48,6 @@
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/pid/pid.h>
 #include <matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -31,7 +31,7 @@
 #
 ############################################################################

-if(${CMAKE_VERSION} VERSION_LESS "3.11")
+if(${CMAKE_VERSION} VERSION_LESS_EQUAL "3.15")
 	message(WARNING "skipping uxrce_dds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}")

 else()
Author	SHA1	Message	Date
Daniel Agar	6f484b652f	[WIP] ekf2: EKF init immediately on first IMU sample - tilt init use filtered accel/gyro on delayed time horizon - publish estimator_status on every EKF update (even if !_filter_initialised)	2024-11-27 17:22:51 -05:00
Daniel Agar	990b067b25	uxrce_dds_client: update cmake requirements to match Micro-XRCE-DDS-Client submodule	2024-11-27 14:09:13 -08:00
Daniel Agar	68cbbaab92	Tools/astyle: check_code_style_all.sh skip pre-commit hook if non-interactive	2024-11-27 13:51:10 -08:00
Ramon Roche	22c1f07f0c	container: use PX4 tags whiel tagging images	2024-11-27 16:42:13 -05:00
Ramon Roche	f2bbb6f407	ci: disable publishing PR images to docker hub Docker hub is rate limiting our API access, as a result tests are failing for no apparent reason. This change will decrease the API calls by at least 80% We have applied for an Open Source account with greater API limits, I will come back to this and update as necessary when and if they grant us access to their program.	2024-11-27 16:42:13 -05:00
bresch	85bc8ef885	SIH-plane: fix actuator mapping	2024-11-27 11:14:56 -05:00
bresch	8a9bac29a2	SIH-FW: allow pitching up during takeoff Otherwise difficult to get lift	2024-11-27 11:14:56 -05:00
bresch	7236ef2d17	SIH-FW: fix aileron and elevator signs This broken when changing from mixer files to the control allocation module.	2024-11-27 11:14:56 -05:00
bresch	1dad25b763	SIH: do not assume being a tailsitter when creating airspeed measurement	2024-11-27 11:14:56 -05:00
Matthias Grob	a280d67be8	PID: Fix test to respect integral updates being applied in the next iteration Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com>	2024-11-26 16:13:48 +01:00
Matthias Grob	f9bcbc31ae	PID: protect from division by zero because of dt Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com>	2024-11-26 16:13:48 +01:00
Matthias Grob	b89c53d28c	Replace old pid library with new one	2024-11-26 16:13:48 +01:00
Matthias Grob	e047972cde	Add new C++ PID library	2024-11-26 16:13:48 +01:00
Daniel Agar	e194a52907	ekf2: derivation.py remove sideslip small angle approximation	2024-11-25 08:53:57 +01:00