clang-tidy fixes, documentation, refactoring.
This commit is contained in:
@@ -22,8 +22,8 @@
|
||||
/// PERFORMANCE OF THIS SOFTWARE.
|
||||
///
|
||||
|
||||
#ifndef SCCCL_MATH_H
|
||||
#define SCCCL_MATH_H
|
||||
#ifndef SCSL_SCMP_MATH_H
|
||||
#define SCSL_SCMP_MATH_H
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
@@ -118,4 +118,4 @@ WithinTolerance(T a, T b, T epsilon)
|
||||
} // namespace scmp
|
||||
|
||||
|
||||
#endif //SCCCL_MATH_H
|
||||
#endif //SCSL_SCMP_MATH_H
|
||||
|
||||
@@ -1,21 +0,0 @@
|
||||
//
|
||||
// Created by Kyle Isom on 2/21/20.
|
||||
//
|
||||
|
||||
#ifndef SCCCL_MOTION2D_H
|
||||
#define SCCCL_MOTION2D_H
|
||||
|
||||
|
||||
#include <scmp/geom/Vector.h>
|
||||
|
||||
namespace scmp {
|
||||
namespace basic {
|
||||
|
||||
|
||||
scmp::geom::Vector2D Acceleration(double speed, double heading);
|
||||
|
||||
|
||||
} // namespace basic
|
||||
} // namespace phsyics
|
||||
|
||||
#endif //SCCCL_MOTION2D_H
|
||||
39
include/scmp/estimation.h
Normal file
39
include/scmp/estimation.h
Normal file
@@ -0,0 +1,39 @@
|
||||
///
|
||||
/// \file include/scmp/estimation.h
|
||||
/// \author K. Isom <kyle@imap.cc>
|
||||
/// \date 2023-10-20
|
||||
/// \brief
|
||||
///
|
||||
/// Copyright 2023 K. Isom <kyle@imap.cc>
|
||||
///
|
||||
/// Permission to use, copy, modify, and/or distribute this software for
|
||||
/// any purpose with or without fee is hereby granted, provided that
|
||||
/// the above copyright notice and this permission notice appear in all /// copies.
|
||||
///
|
||||
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
|
||||
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
|
||||
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
|
||||
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
|
||||
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
|
||||
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
|
||||
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
|
||||
/// PERFORMANCE OF THIS SOFTWARE.
|
||||
///
|
||||
|
||||
#include <scmp/estimation/Madgwick.h>
|
||||
|
||||
|
||||
#ifndef SCSL_ESTIMATION_H
|
||||
#define SCSL_ESTIMATION_H
|
||||
|
||||
namespace scmp {
|
||||
|
||||
|
||||
/// \brief Algorithms for estimation position, and system state.
|
||||
namespace estimation {}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif // SCSL_ESTIMATION_H
|
||||
@@ -1,8 +1,8 @@
|
||||
///
|
||||
/// \file include/scmp/filter/Madgwick.h
|
||||
/// \file include/scmp/estimation/Madgwick.h
|
||||
/// \author K. Isom <kyle@imap.cc>
|
||||
/// \date 2019-08-06
|
||||
/// \brief Implementation of a Madgwick filter.
|
||||
/// \brief Implementation of a Madgwick estimation.
|
||||
///
|
||||
/// See https://courses.cs.washington.edu/courses/cse466/14au/labs/l4/madgwick_internal_report.pdf.
|
||||
///
|
||||
@@ -33,29 +33,29 @@
|
||||
/// scmp contains the chimmering clarity math and physics code.
|
||||
namespace scmp {
|
||||
|
||||
/// filter contains filtering algorithms.
|
||||
namespace filter {
|
||||
namespace estimation {
|
||||
|
||||
|
||||
/// @brief Madgwick implements an efficient Orientation filter for IMUs.
|
||||
/// \brief Madgwick implements an efficient Orientation estimation for
|
||||
/// Intertial Measurement Units (IMUs).
|
||||
///
|
||||
/// Madgwick is a novel Orientation filter applicable to IMUs
|
||||
/// Madgwick is a novel Orientation estimation applicable to IMUs
|
||||
/// consisting of tri-Axis gyroscopes and accelerometers, and MARG
|
||||
/// sensor arrays that also include tri-Axis magnetometers. The MARG
|
||||
/// implementation incorporates magnetic distortionand gyroscope bias
|
||||
/// drift compensation.
|
||||
///
|
||||
/// It is described in the paper [An efficient Orientation filter for inertial and inertial/magnetic sensor arrays](http://x-io.co.uk/res/doc/madgwick_internal_report.pdf).
|
||||
/// It is described in the paper [An efficient Orientation estimation for inertial and inertial/magnetic sensor arrays](http://x-io.co.uk/res/doc/madgwick_internal_report.pdf).
|
||||
///
|
||||
/// \tparam T A floating point type.
|
||||
template <typename T>
|
||||
class Madgwick {
|
||||
public:
|
||||
/// \brief The Madgwick filter is initialised with an identity MakeQuaternion.
|
||||
/// \brief The Madgwick estimation is initialised with an identity MakeQuaternion.
|
||||
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame()
|
||||
{};
|
||||
|
||||
/// \brief The Madgwick filter is initialised with a sensor frame.
|
||||
/// \brief The Madgwick estimation is initialised with a sensor frame.
|
||||
///
|
||||
/// \param sf A sensor frame; if zero, the sensor frame will be
|
||||
/// initialised as an identity MakeQuaternion.
|
||||
@@ -66,7 +66,7 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
/// \brief Initialise the filter with a sensor frame MakeQuaternion.
|
||||
/// \brief Initialise the estimation with a sensor frame MakeQuaternion.
|
||||
///
|
||||
/// \param sf A MakeQuaternion representing the current Orientation.
|
||||
Madgwick(scmp::geom::Quaternion<T> sf) :
|
||||
@@ -74,7 +74,7 @@ public:
|
||||
{};
|
||||
|
||||
/// \brief Return the current orientation as measured by the
|
||||
/// filter.
|
||||
/// estimation.
|
||||
///
|
||||
/// \return The current sensor frame.
|
||||
scmp::geom::Quaternion<T>
|
||||
@@ -83,15 +83,16 @@ public:
|
||||
return this->sensorFrame;
|
||||
}
|
||||
|
||||
/// \brief Return the filter's rate of angular change from a
|
||||
/// \brief Return the estimation's rate of angular change from a
|
||||
/// sensor frame.
|
||||
///
|
||||
/// Return the rate of change of the Orientation of the earth frame
|
||||
/// with respect to the sensor frame.
|
||||
/// Return the rate of change of the Orientation of the earth
|
||||
/// frame with respect to the sensor frame.
|
||||
///
|
||||
/// \param gyro A three-dimensional vector containing gyro readings
|
||||
/// as w_x, w_y, w_z.
|
||||
/// \return A MakeQuaternion representing the rate of angular change.
|
||||
/// \param gyro A three-dimensional vector containing gyro
|
||||
/// readings as w_x, w_y, w_z.
|
||||
/// \return A MakeQuaternion representing the rate of angular
|
||||
/// change.
|
||||
scmp::geom::Quaternion<T>
|
||||
AngularRate(const scmp::geom::Vector<T, 3> &gyro) const
|
||||
{
|
||||
@@ -112,8 +113,8 @@ public:
|
||||
|
||||
/// \brief Update the sensor frame to a new frame.
|
||||
///
|
||||
/// \warning The filter's default Δt must be set before calling
|
||||
// this.
|
||||
/// \warning The estimation's default Δt must be set before
|
||||
/// calling this.
|
||||
///
|
||||
/// \param sf The new sensor frame replacing the previous one.
|
||||
void
|
||||
@@ -129,9 +130,10 @@ public:
|
||||
/// the compile flag NDEBUG, but may be useful to catch
|
||||
/// possible errors.
|
||||
///
|
||||
/// \param gyro A three-dimensional vector containing gyro readings
|
||||
/// as w_x, w_y, w_z.
|
||||
/// \param delta The time step between readings. It must not be zero.
|
||||
/// \param gyro A three-dimensional vector containing gyro
|
||||
/// readings as w_x, w_y, w_z.
|
||||
/// \param delta The time step between readings. It must not
|
||||
/// be zero.
|
||||
void
|
||||
UpdateAngularOrientation(const scmp::geom::Vector<T, 3> &gyro, T delta)
|
||||
{
|
||||
@@ -147,20 +149,21 @@ public:
|
||||
|
||||
/// \brief Update the sensor frame with a gyroscope reading.
|
||||
///
|
||||
/// If no Δt is provided, the filter's default is used.
|
||||
/// If no Δt is provided, the estimation's default is used.
|
||||
///
|
||||
/// \warning The default Δt must be explicitly set using DeltaT
|
||||
/// before calling this.
|
||||
///
|
||||
/// \param gyro A three-dimensional vector containing gyro readings
|
||||
/// as w_x, w_y, w_z.
|
||||
/// \param gyro A three-dimensional vector containing gyro
|
||||
/// readings as w_x, w_y, w_z.
|
||||
void
|
||||
UpdateAngularOrientation(const scmp::geom::Vector<T, 3> &gyro)
|
||||
{
|
||||
this->UpdateAngularOrientation(gyro, this->deltaT);
|
||||
}
|
||||
|
||||
/// \brief Retrieve a vector of the Euler angles in ZYX Orientation.
|
||||
/// \brief Retrieve a vector of the Euler angles in ZYX
|
||||
/// Orientation.
|
||||
///
|
||||
/// \return A vector of Euler angles as <ψ, θ, ϕ>.
|
||||
scmp::geom::Vector<T, 3>
|
||||
@@ -172,19 +175,19 @@ public:
|
||||
/// \brief Set the default Δt.
|
||||
///
|
||||
/// \note This must be explicitly called before calling any
|
||||
/// method which uses the filter's internal Δt.
|
||||
/// method which uses the estimation's internal Δt.
|
||||
///
|
||||
/// \param newDeltaT The time delta to use when no time delta is
|
||||
/// provided.
|
||||
/// \param newDeltaT The time delta to use when no time delta
|
||||
/// is provided.
|
||||
void
|
||||
DeltaT(T newDeltaT)
|
||||
{
|
||||
this->deltaT = newDeltaT;
|
||||
}
|
||||
|
||||
/// \brief Retrieve the filter's current ΔT.
|
||||
/// \brief Retrieve the estimation's current ΔT.
|
||||
///
|
||||
/// \return The current value the filter will default to using
|
||||
/// \return The current value the estimation will default to using
|
||||
/// if no time delta is provided.
|
||||
T DeltaT() { return this->deltaT; }
|
||||
|
||||
@@ -202,7 +205,7 @@ using Madgwickd = Madgwick<double>;
|
||||
using Madgwickf = Madgwick<float>;
|
||||
|
||||
|
||||
} // namespace filter
|
||||
} // namespace estimation
|
||||
} // namespace scmp
|
||||
|
||||
|
||||
@@ -91,19 +91,19 @@ float Heading2F(Vector2F vec);
|
||||
///
|
||||
/// \param vec A vector Orientation.
|
||||
/// \return The compass heading of the vector in radians.
|
||||
double Heading2d(Vector2D vec);
|
||||
double Heading2D(Vector2D vec);
|
||||
|
||||
/// \brief Compass heading for a Vector2F.
|
||||
///
|
||||
/// \param vec A vector Orientation.
|
||||
/// \return The compass heading of the vector in radians.
|
||||
float Heading3f(Vector3F vec);
|
||||
float Heading3F(Vector3F vec);
|
||||
|
||||
/// Heading3d returns a compass heading for a Vector2F.
|
||||
/// Heading3D returns a compass heading for a Vector2F.
|
||||
///
|
||||
/// \param vec A vector Orientation.
|
||||
/// \return The compass heading of the vector in radians.
|
||||
double Heading3d(Vector3D vec);
|
||||
double Heading3D(Vector3D vec);
|
||||
|
||||
|
||||
} // namespace geom
|
||||
|
||||
Reference in New Issue
Block a user