/*
 * madgwick.cpp
 *
 *  Created on: Jun 20, 2024
 *      Author: Dario
 */

#include <sta/math/algorithms/attitude/madgwick.hpp>

namespace sta
{
	namespace math
	{
		MadgwickFilter::MadgwickFilter(Quaternion state, uint32_t n, float alpha)
			: state_{state.normalized()},
			  n_{n},
			  alpha_{alpha}
		{

		}

		void MadgwickFilter::predict(float dt, float ox, float oy, float oz)
		{
			state_ = state_ + 0.5f * (state_ * Quaternion(0, ox, oy, oz)) * dt;
		}

		void MadgwickFilter::correct(float dx, float dy, float dz)
		{

		}


	}  // namespace math
}  // namespace sta