fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-25 05:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Automatic Differentiation 'Dual' Type (#100)

Browse Source 
* Dual numbers initial implementation

* Add test coverage, with partial derivative example

* Add Jacobian test, fix small issues

* Improve test to demonstrate non-square jacobian

* Better naming for collectReals/Derivatives

* Improve comments

* Potential GCC 4.8 bug workaround

* Add fallback workaround for non-IEEE float platforms
This commit is contained in:
Julian Kent
2019-10-23 12:07:51 +02:00
committed by GitHub
parent 92d1c8761e
commit 215203fc6f
7 changed files with 699 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Dual.hpp
+355
View File
@@ -0,0 +1,355 @@
/**
* @file Dual.hpp
*
* This is a dual number type for calculating automatic derivatives.
*
* Based roughly on the methods described in:
* Automatic Differentiation, C++ Templates and Photogrammetry, by Dan Piponi
* and
* Ceres Solver's excellent Jet.h
*
* @author Julian Kent <julian@auterion.com>
*/
#pragma once
#include "math.hpp"
namespace matrix
{
template <typename Type, size_t M>
class Vector;
template <typename Scalar, size_t N>
struct Dual
{
static constexpr size_t WIDTH = N;
Scalar value {};
Vector<Scalar, N> derivative;
Dual() = default;
explicit Dual(Scalar v, size_t inputDimension = 65535)
{
value = v;
if (inputDimension < N) {
derivative(inputDimension) = Scalar(1);
}
}
explicit Dual(Scalar v, const Vector<Scalar, N>& d) :
value(v), derivative(d)
{}
Dual<Scalar, N>& operator=(const Scalar& a)
{
derivative.setZero();
value = a;
return *this;
}
Dual<Scalar, N>& operator +=(const Dual<Scalar, N>& a)
{
return (*this = *this + a);
}
Dual<Scalar, N>& operator *=(const Dual<Scalar, N>& a)
{
return (*this = *this * a);
}
Dual<Scalar, N>& operator -=(const Dual<Scalar, N>& a)
{
return (*this = *this - a);
}
Dual<Scalar, N>& operator /=(const Dual<Scalar, N>& a)
{
return (*this = *this / a);
}
Dual<Scalar, N>& operator +=(Scalar a)
{
return (*this = *this + a);
}
Dual<Scalar, N>& operator -=(Scalar a)
{
return (*this = *this - a);
}
Dual<Scalar, N>& operator *=(Scalar a)
{
return (*this = *this * a);
}
Dual<Scalar, N>& operator /=(Scalar a)
{
return (*this = *this / a);
}
};
// operators
template <typename Scalar, size_t N>
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a)
{
return a;
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a)
{
return Dual<Scalar, N>(-a.value, -a.derivative);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
{
return Dual<Scalar, N>(a.value + b.value, a.derivative + b.derivative);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
{
return a + (-b);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, Scalar b)
{
return Dual<Scalar, N>(a.value + b, a.derivative);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, Scalar b)
{
return a + (-b);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator+(Scalar a, const Dual<Scalar, N>& b)
{
return Dual<Scalar, N>(a + b.value, b.derivative);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator-(Scalar a, const Dual<Scalar, N>& b)
{
return a + (-b);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
{
return Dual<Scalar, N>(a.value * b.value, a.value * b.derivative + b.value * a.derivative);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, Scalar b)
{
return Dual<Scalar, N>(a.value * b, a.derivative * b);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator*(Scalar a, const Dual<Scalar, N>& b)
{
return b * a;
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
{
const Scalar inv_b_real = Scalar(1) / b.value;
return Dual<Scalar, N>(a.value * inv_b_real, a.derivative * inv_b_real -
a.value * b.derivative * inv_b_real * inv_b_real);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, Scalar b)
{
return a * (Scalar(1) / b);
}
template <typename Scalar, size_t N>
Dual<Scalar, N> operator/(Scalar a, const Dual<Scalar, N>& b)
{
const Scalar inv_b_real = Scalar(1) / b.value;
return Dual<Scalar, N>(a * inv_b_real, (-inv_b_real * a * inv_b_real) * b.derivative);
}
// basic math
// sqrt
template <typename Scalar, size_t N>
Dual<Scalar, N> sqrt(const Dual<Scalar, N>& a)
{
Scalar real = sqrt(a.value);
return Dual<Scalar, N>(real, a.derivative * (Scalar(1) / (Scalar(2) * real)));
}
// abs
template <typename Scalar, size_t N>
Dual<Scalar, N> abs(const Dual<Scalar, N>& a)
{
return a.value >= Scalar(0) ? a : -a;
}
// ceil
template <typename Scalar, size_t N>
Dual<Scalar, N> ceil(const Dual<Scalar, N>& a)
{
return Dual<Scalar, N>(ceil(a.value));
}
// floor
template <typename Scalar, size_t N>
Dual<Scalar, N> floor(const Dual<Scalar, N>& a)
{
return Dual<Scalar, N>(floor(a.value));
}
// fmod
template <typename Scalar, size_t N>
Dual<Scalar, N> fmod(const Dual<Scalar, N>& a, Scalar mod)
{
return Dual<Scalar, N>(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative);
}
// max
template <typename Scalar, size_t N>
Dual<Scalar, N> max(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
{
return a.value >= b.value ? a : b;
}
// min
template <typename Scalar, size_t N>
Dual<Scalar, N> min(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
{
return a.value < b.value ? a : b;
}
// isnan
template <typename Scalar, size_t N>
bool isnan(const Dual<Scalar, N>& a)
{
return isnan(a.value);
}
// isfinite
template <typename Scalar, size_t N>
bool isfinite(const Dual<Scalar, N>& a)
{
return isfinite(a.value);
}
// isinf
template <typename Scalar, size_t N>
bool isinf(const Dual<Scalar, N>& a)
{
return isinf(a.value);
}
// trig
// sin
template <typename Scalar, size_t N>
Dual<Scalar, N> sin(const Dual<Scalar, N>& a)
{
return Dual<Scalar, N>(sin(a.value), cos(a.value) * a.derivative);
}
// cos
template <typename Scalar, size_t N>
Dual<Scalar, N> cos(const Dual<Scalar, N>& a)
{
return Dual<Scalar, N>(cos(a.value), -sin(a.value) * a.derivative);
}
// tan
template <typename Scalar, size_t N>
Dual<Scalar, N> tan(const Dual<Scalar, N>& a)
{
Scalar real = tan(a.value);
return Dual<Scalar, N>(real, (Scalar(1) + real * real) * a.derivative);
}
// asin
template <typename Scalar, size_t N>
Dual<Scalar, N> asin(const Dual<Scalar, N>& a)
{
Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative);
}
// acos
template <typename Scalar, size_t N>
Dual<Scalar, N> acos(const Dual<Scalar, N>& a)
{
Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative);
}
// atan
template <typename Scalar, size_t N>
Dual<Scalar, N> atan(const Dual<Scalar, N>& a)
{
Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value);
return Dual<Scalar, N>(atan(a.value), atan_d * a.derivative);
}
// atan2
template <typename Scalar, size_t N>
Dual<Scalar, N> atan2(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
{
// derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify
Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value);
return Dual<Scalar, N>(atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d);
}
// retrieve the derivative elements of a vector of Duals into a matrix
template <typename Scalar, size_t M, size_t N>
Matrix<Scalar, M, N> collectDerivatives(const Matrix<Dual<Scalar, N>, M, 1>& input)
{
Matrix<Scalar, M, N> jac;
for (size_t i = 0; i < M; i++) {
jac.row(i) = input(i, 0).derivative;
}
return jac;
}
// retrieve the real (non-derivative) elements of a matrix of Duals into an equally sized matrix
template <typename Scalar, size_t M, size_t N, size_t D>
Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N>& input)
{
Matrix<Scalar, M, N> r;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
r(i,j) = input(i,j).value;
}
}
return r;
}
#if defined(SUPPORT_STDIOSTREAM)
template<typename Type, size_t N>
std::ostream& operator<<(std::ostream& os,
const matrix::Dual<Type, N>& dual)
{
os << "[";
os << std::setw(10) << dual.value << ";";
for (size_t j = 0; j < N; ++j) {
os << "\t";
os << std::setw(10) << static_cast<double>(dual.derivative(j));
}
os << "]";
return os;
}
#endif // defined(SUPPORT_STDIOSTREAM)
}
matrix/Matrix.hpp
+22 -4
View File
@@ -34,12 +34,23 @@ template<typename Type, size_t M, size_t N>
class Matrix
{
Type _data[M][N] {};
Type _data[M][N];
public:
// Constructors
Matrix() = default;
Matrix()
{
#ifdef __STDC_IEC_559__
memset(_data, 0, sizeof(_data)); //workaround for GCC 4.8 bug, don't use {} for array init
#else
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
_data[i][j] = Type{};
}
}
#endif
}
explicit Matrix(const Type data_[M*N])
{
@@ -558,7 +569,14 @@ Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
template<typename Type, size_t M, size_t N>
bool isEqual(const Matrix<Type, M, N> &x,
const Matrix<Type, M, N> &y, const Type eps=1e-4f) {
return x == y;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (!isEqualF(x(i,j), y(i,j), eps)) {
return false;
}
}
}
return true;
}
#if defined(SUPPORT_STDIOSTREAM)
@@ -569,7 +587,7 @@ std::ostream& operator<<(std::ostream& os,
for (size_t i = 0; i < M; ++i) {
os << "[";
for (size_t j = 0; j < N; ++j) {
os << std::setw(10) << static_cast<double>(matrix(i, j));
os << std::setw(10) << matrix(i, j);
os << "\t";
}
os << "]" << std::endl;
matrix/Quaternion.hpp
+8 -8
View File
@@ -395,11 +395,11 @@ public:
* @param vec vector to rotate in frame 1 (typically body frame)
* @return rotated vector in frame 2 (typically reference frame)
*/
Vector3f conjugate(const Vector3f &vec) {
Quaternion q = *this;
Quaternion v(0, vec(0), vec(1), vec(2));
Vector3<Type> conjugate(const Vector3<Type> &vec) const {
const Quaternion& q = *this;
Quaternion v(Type(0), vec(0), vec(1), vec(2));
Quaternion res = q*v*q.inversed();
return Vector3f(res(1), res(2), res(3));
return Vector3<Type>(res(1), res(2), res(3));
}
/**
@@ -411,12 +411,12 @@ public:
* @param vec vector to rotate in frame 2 (typically reference frame)
* @return rotated vector in frame 1 (typically body frame)
*/
Vector3f conjugate_inversed(const Vector3f &vec) const
Vector3<Type> conjugate_inversed(const Vector3<Type> &vec) const
{
Quaternion q = *this;
Quaternion v(0, vec(0), vec(1), vec(2));
const Quaternion& q = *this;
Quaternion v(Type(0), vec(0), vec(1), vec(2));
Quaternion res = q.inversed()*v*q;
return Vector3f(res(1), res(2), res(3));
return Vector3<Type>(res(1), res(2), res(3));
}
/**
matrix/Vector.hpp
+2 -2
View File
@@ -54,7 +54,7 @@ public:
Type dot(const MatrixM1 & b) const {
const Vector &a(*this);
Type r = 0;
Type r(0);
for (size_t i = 0; i<M; i++) {
r += a(i)*b(i,0);
}
@@ -66,7 +66,7 @@ public:
return a.dot(b);
}
inline Vector operator*(float b) const {
inline Vector operator*(Type b) const {
return Vector(MatrixM1::operator*(b));
}
matrix/math.hpp
+1
View File
@@ -17,3 +17,4 @@
#include "Quaternion.hpp"
#include "AxisAngle.hpp"
#include "LeastSquaresSolver.hpp"
#include "Dual.hpp"
test/CMakeLists.txt
+1
View File
@@ -19,6 +19,7 @@ set(tests
copyto
least_squares
upperRightTriangle
dual
)
add_custom_target(test_build)
test/dual.cpp
+310
View File
@@ -0,0 +1,310 @@
#include "test_macros.hpp"
#include <matrix/math.hpp>
#include <iostream>
using namespace matrix;
template <typename Scalar, size_t N>
bool isEqualAll(Dual<Scalar, N> a, Dual<Scalar, N> b)
{
return isEqualF(a.value, b.value) && a.derivative == b.derivative;
}
template <typename T>
T testFunction(const Vector<T, 3>& point) {
// function is f(x,y,z) = x^2 + 2xy + 3y^2 + z
return point(0)*point(0)
+ 2.f * point(0) * point(1)
+ 3.f * point(1) * point(1)
+ point(2);
}
template <typename Scalar>
Vector<Scalar, 3> positionError(const Vector<Scalar, 3>& positionState,
const Vector<Scalar, 3>& velocityStateBody,
const Quaternion<Scalar>& bodyOrientation,
const Vector<Scalar, 3>& positionMeasurement,
Scalar dt
)
{
return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt);
}
int main()
{
const Dual<float, 1> a(3,0);
const Dual<float, 1> b(6,0);
{
TEST(isEqualF(a.value, 3.f));
TEST(isEqualF(a.derivative(0), 1.f));
}
{
// addition
Dual<float, 1> c = a + b;
TEST(isEqualF(c.value, 9.f));
TEST(isEqualF(c.derivative(0), 2.f));
Dual<float, 1> d = +a;
TEST(isEqualAll(d, a));
d += b;
TEST(isEqualAll(d, c));
Dual<float, 1> e = a;
e += b.value;
TEST(isEqualF(e.value, c.value));
TEST(isEqual(e.derivative, a.derivative));
Dual<float, 1> f = b.value + a;
TEST(isEqualAll(f, e));
}
{
// subtraction
Dual<float, 1> c = b - a;
TEST(isEqualF(c.value, 3.f));
TEST(isEqualF(c.derivative(0), 0.f));
Dual<float, 1> d = b;
TEST(isEqualAll(d, b));
d -= a;
TEST(isEqualAll(d, c));
Dual<float, 1> e = b;
e -= a.value;
TEST(isEqualF(e.value, c.value));
TEST(isEqual(e.derivative, b.derivative));
Dual<float, 1> f = a.value - b;
TEST(isEqualAll(f, -e));
}
{
// multiplication
Dual<float, 1> c = a*b;
TEST(isEqualF(c.value, 18.f));
TEST(isEqualF(c.derivative(0), 9.f));
Dual<float, 1> d = a;
TEST(isEqualAll(d, a));
d *= b;
TEST(isEqualAll(d, c));
Dual<float, 1> e = a;
e *= b.value;
TEST(isEqualF(e.value, c.value));
TEST(isEqual(e.derivative, a.derivative * b.value));
Dual<float, 1> f = b.value * a;
TEST(isEqualAll(f, e));
}
{
// division
Dual<float, 1> c = b/a;
TEST(isEqualF(c.value, 2.f));
TEST(isEqualF(c.derivative(0), -1.f/3.f));
Dual<float, 1> d = b;
TEST(isEqualAll(d, b));
d /= a;
TEST(isEqualAll(d, c));
Dual<float, 1> e = b;
e /= a.value;
TEST(isEqualF(e.value, c.value));
TEST(isEqual(e.derivative, b.derivative / a.value));
Dual<float, 1> f = a.value / b;
TEST(isEqualAll(f, 1.f/e));
}
{
Dual<float, 1> blank;
TEST(isEqualF(blank.value, 0.f));
TEST(isEqualF(blank.derivative(0), 0.f));
}
{
// sqrt
TEST(isEqualF(sqrt(a).value, sqrt(a.value)));
TEST(isEqualF(sqrt(a).derivative(0), 1.f/sqrt(12.f)));
}
{
// abs
TEST(isEqualAll(a, abs(-a)));
TEST(!isEqualAll(-a, abs(a)));
TEST(isEqualAll(-a, -abs(a)));
}
{
// ceil
Dual<float, 1> c(1.5,0);
TEST(isEqualF(ceil(c).value, ceil(c.value)));
TEST(isEqualF(ceil(c).derivative(0), 0.f));
}
{
// floor
Dual<float, 1> c(1.5,0);
TEST(isEqualF(floor(c).value, floor(c.value)));
TEST(isEqualF(floor(c).derivative(0), 0.f));
}
{
// fmod
TEST(isEqualF(fmod(a, 0.8f).value, fmod(a.value, 0.8f)));
TEST(isEqual(fmod(a, 0.8f).derivative, a.derivative));
}
{
// max/min
TEST(isEqualAll(b, max(a, b)));
TEST(isEqualAll(a, min(a, b)));
}
{
// isnan
TEST(!isnan(a));
Dual<float, 1> c(sqrt(-1.f),0);
TEST(isnan(c));
}
{
// isfinite/isinf
TEST(isfinite(a));
TEST(!isinf(a));
Dual<float, 1> c(sqrt(-1.f),0);
TEST(!isfinite(c));
TEST(!isinf(c));
Dual<float, 1> d(INFINITY,0);
TEST(!isfinite(d));
TEST(isinf(d));
}
{
// sin/cos/tan
TEST(isEqualF(sin(a).value, sin(a.value)));
TEST(isEqualF(sin(a).derivative(0), cos(a.value))); // sin'(x) = cos(x)
TEST(isEqualF(cos(a).value, cos(a.value)));
TEST(isEqualF(cos(a).derivative(0), -sin(a.value))); // cos'(x) = -sin(x)
TEST(isEqualF(tan(a).value, tan(a.value)));
TEST(isEqualF(tan(a).derivative(0), 1.f + tan(a.value)*tan(a.value))); // tan'(x) = 1 + tan^2(x)
}
{
// asin/acos/atan
Dual<float, 1> c(0.3f, 0);
TEST(isEqualF(asin(c).value, asin(c.value)));
TEST(isEqualF(asin(c).derivative(0), 1.f/sqrt(1.f - 0.3f*0.3f))); // asin'(x) = 1/sqrt(1-x^2)
TEST(isEqualF(acos(c).value, acos(c.value)));
TEST(isEqualF(acos(c).derivative(0), -1.f/sqrt(1.f - 0.3f*0.3f))); // acos'(x) = -1/sqrt(1-x^2)
TEST(isEqualF(atan(c).value, atan(c.value)));
TEST(isEqualF(atan(c).derivative(0), 1.f/(1.f + 0.3f*0.3f))); // tan'(x) = 1 + x^2
}
{
// atan2
TEST(isEqualF(atan2(a, b).value, atan2(a.value, b.value)));
TEST(isEqualAll(atan2(a, Dual<float,1>(b.value)), atan(a/b.value))); // atan2'(y, x) = atan'(y/x)
}
{
// partial derivatives
// function is f(x,y,z) = x^2 + 2xy + 3y^2 + z, we need with respect to d/dx and d/dy at the point (0.5, -0.8, 2)
using D = Dual<float, 2>;
// set our starting point, requesting partial derivatives of x and y in column 0 and 1
Vector3<D> dualPoint(D(0.5f, 0), D(-0.8f, 1), D(2.f));
Dual<float, 2> dualResult = testFunction(dualPoint);
// compare to a numerical derivative:
Vector<float, 3> floatPoint = collectReals(dualPoint);
float floatResult = testFunction(floatPoint);
float h = 0.0001f;
Vector<float, 3> floatPoint_plusDX = floatPoint;
floatPoint_plusDX(0) += h;
float floatResult_plusDX = testFunction(floatPoint_plusDX);
Vector<float, 3> floatPoint_plusDY = floatPoint;
floatPoint_plusDY(1) += h;
float floatResult_plusDY = testFunction(floatPoint_plusDY);
Vector2f numerical_derivative((floatResult_plusDX - floatResult)/h,
(floatResult_plusDY - floatResult)/h);
TEST(isEqualF(dualResult.value, floatResult, 0.0f));
TEST(isEqual(dualResult.derivative, numerical_derivative, 1e-2f));
}
{
// jacobian
// get residual of x/y/z with partial derivatives of rotation
Vector3f direct_error;
Matrix<float, 3, 4> numerical_jacobian;
{
Vector3f positionState(5,6,7);
Vector3f velocityState(-1,0,1);
Quaternionf velocityOrientation(0.2f,-0.1f,0,1);
Vector3f positionMeasurement(4.5f, 6.2f, 7.9f);
float dt = 0.1f;
direct_error = positionError(positionState,
velocityState,
velocityOrientation,
positionMeasurement,
dt);
float h = 0.001f;
for (size_t i = 0; i < 4; i++)
{
Quaternion<float> h4 = velocityOrientation;
h4(i) += h;
numerical_jacobian.col(i) = (positionError(positionState,
velocityState,
h4,
positionMeasurement,
dt)
- direct_error)/h;
}
}
Vector3f auto_error;
Matrix<float, 3, 4> auto_jacobian;
{
using D4 = Dual<float, 4>;
using Vector3d4 = Vector3<D4>;
Vector3d4 positionState(D4(5), D4(6), D4(7));
Vector3d4 velocityState(D4(-1), D4(0), D4(1));
// request partial derivatives of velocity orientation
// by setting these variables' derivatives in corresponding columns [0...3]
Quaternion<D4> velocityOrientation(D4(0.2f, 0),D4(-0.1f, 1),D4(0, 2),D4(1, 3));
Vector3d4 positionMeasurement(D4(4.5f), D4(6.2f), D4(7.9f));
D4 dt(0.1f);
Vector3d4 error = positionError(positionState,
velocityState,
velocityOrientation,
positionMeasurement,
dt);
auto_error = collectReals(error);
auto_jacobian = collectDerivatives(error);
}
TEST(isEqual(direct_error, auto_error, 0.0f));
TEST(isEqual(numerical_jacobian, auto_jacobian, 1e-3f));
}
return 0;
}