Control system with object simulation

controller.cpp

#include "controller.hpp"
#include "modelfilter.hpp"

namespace controller
{
	vector< quantity, 2 > targetpos = { 0.0_m, 0.0_m };

	void update()
	{
		vector< quantity, 2 > posdiff = targetpos - modelfilter::ballpos;
		vector< quantity, 2 > targetvel = 10.0_ps * posdiff;
		vector< quantity, 2 > veldiff = targetvel - modelfilter::ballvel;
		modelfilter::targetslope = ( - 0.5_spm ) * veldiff;
	}
}

controller.hpp

#pragma once

#include "units.hpp"
#include "lin.hpp"

namespace controller
{
	extern vector< quantity, 2 > targetpos;
	void update();
};

externals-model.cpp

#include "externals.hpp"
#include "sim.hpp"
#include "lin.hpp"
#include "units.hpp"

namespace externals
{
	vector< quantity, 2 > targetmotorpos = { 0.0_m, 0.0_m };
	vector< quantity, 2 > ballpos = { 0.0_m, 0.0_m };
	int ballposindex = 0;
	vector< quantity, 2 > motorpos = { 0.0_u, 0.0_u };
	vector< quantity, 2 > motorposoffset = { 0.2_u, -0.5_u };
	// vector< quantity, 2 > motorposoffset = { 0.0_u, -0.0_u };

	void update()
	{
		sim::targetmotorpos( 0 ) =
			sim::quantity( ( long double )(
				targetmotorpos( 0 ) + motorposoffset( 0 ) ) );
		sim::targetmotorpos( 1 ) =
			sim::quantity( ( long double )(
				targetmotorpos( 1 ) + motorposoffset( 1 ) ) );
		for( int i = 0; i < sim::timefactor; ++i )
		{
			sim::step();
		}
		sim::measurementtime -= 1.0_u;
		if( sim::measurementtime <= 0.0_u )
		{
			sim::measurementtime += sim::measurementinterval;
			ballpos( 0 ) =
				quantity( unit::m / sim::m * ( long double )sim::ballpos( 0 ) );
			ballpos( 1 ) =
				quantity( unit::m / sim::m * ( long double )sim::ballpos( 1 ) );
			ballposindex += 1;
		}
		motorpos = sim::motorpos - motorposoffset;
	}
}

externals.hpp

#pragma once

#include "units.hpp"
#include "lin.hpp"

namespace externals
{
	extern vector< quantity, 2 > targetmotorpos;
	extern vector< quantity, 2 > ballpos;
	extern int ballposindex;
	extern vector< quantity, 2 > motorpos;
	extern vector< quantity, 2 > motorposoffset;
	void update();
};

lin.hpp

#pragma once

#include <array>

template< typename T >
struct linear_traits
{
};

template< typename T, size_t size >
struct vector
{
	std::array< T, size > elems;

	T& operator()( size_t index )
	{
		return elems[ index ];
	}

	T const& operator()( size_t index ) const
	{
		return elems[ index ];
	}
};

template< typename T, size_t size >
struct linear_traits< vector< T, size > >
{
	using elemtype = T;
	using objtype = vector< T, size >;
	template< typename U >
	using common =
		linear_traits< vector< decltype( T{} * U{} ), size > >;

	template< typename U >
	struct with
	{
	};

	template< typename U >
	struct with< vector< U, size > >
	{
		using valid = void;
		using elemtype = decltype( T{} + U{} );
		using common = linear_traits< vector< elemtype, size > >;
	};

	static constexpr size_t dims = size;

	static T& elem( objtype& v, size_t index )
	{
		return v.elems[ index ];
	}

	static T const& elem( objtype const& v, size_t index )
	{
		return v.elems[ index ];
	}
};

template< typename T, size_t asize, size_t bsize >
struct matrix
{
	std::array< T, asize * bsize > elems;

	T& operator()( size_t ai, size_t bi )
	{
		return elems[ ai * bsize + bi ];
	}

	T const& operator()( size_t ai, size_t bi ) const
	{
		return elems[ ai * bsize + bi ];
	}
};

template< typename T, size_t asize, size_t bsize >
struct linear_traits< matrix< T, asize, bsize > >
{
	using elemtype = T;
	using objtype = matrix< T, asize, bsize >;
	template< typename U >
	using common =
		linear_traits< matrix< decltype( T{} * U{} ), asize, bsize > >;

	template< typename U >
	struct with
	{
	};

	template< typename U >
	struct with< matrix< U, asize, bsize > >
	{
		using valid = void;
		using elemtype = decltype( T{} + U{} );
		using common = linear_traits< matrix< elemtype, asize, bsize > >;
	};

	static constexpr size_t dims = asize * bsize;

	static T& elem( objtype& v, size_t index )
	{
		return v.elems[ index ];
	}

	static T const& elem( objtype const& v, size_t index )
	{
		return v.elems[ index ];
	}
};

template<
	typename A, typename B,
	typename Alin = linear_traits< A >,
	typename Blin = linear_traits< B >,
	typename = typename Alin::template with< B >::valid >
A& addassign(
	A& a,
	B const& b )
{
	for( size_t i = 0; i < Alin::dims; ++i )
	{
		Alin::elem( a, i ) += Blin::elem( b, i );
	}
	return a;
}

template<
	typename A, typename B,
	typename Alin = linear_traits< A >,
	typename Blin = linear_traits< B >,
	typename = typename Alin::template with< B >::valid >
A& subassign(
	A& a,
	B const& b )
{
	for( size_t i = 0; i < Alin::dims; ++i )
	{
		Alin::elem( a, i ) -= Blin::elem( b, i );
	}
	return a;
}

template<
	typename A, typename B,
	typename Alin = linear_traits< A > >
A& scaleassign(
	A& a,
	B const& b )
{
	for( size_t i = 0; i < Alin::dims; ++i )
	{
		Alin::elem( a, i ) *= b;
	}
	return a;
}

template<
	typename A, typename B,
	typename Alin = linear_traits< A > >
A& invscaleassign(
	A& a,
	B const& b )
{
	for( size_t i = 0; i < Alin::dims; ++i )
	{
		Alin::elem( a, i ) /= b;
	}
	return a;
}

template<
	typename A,
	typename Alin = linear_traits< A > >
A invert(
	A const& a )
{
	A r;
	for( size_t i = 0; i < Alin::dims; ++i )
	{
		Alin::elem( r, i ) = - Alin::elem( a, i );
	}
}

template<
	typename A, typename B,
	typename Alin = linear_traits< A >,
	typename Blin = linear_traits< B >,
	typename Rlin = typename Alin::template with< B >::common >
typename Rlin::objtype add(
	A const& a,
	B const& b )
{
	typename Rlin::objtype r;
	for( size_t i = 0; i < Rlin::dims; ++i )
	{
		Rlin::elem( r, i ) = Alin::elem( a, i ) + Blin::elem( b, i );
	}
	return r;
}

template<
	typename A, typename B,
	typename Alin = linear_traits< A >,
	typename Blin = linear_traits< B >,
	typename Rlin = typename Alin::template with< B >::common >
typename Rlin::objtype sub(
	A const& a,
	B const& b )
{
	typename Rlin::objtype r;
	for( size_t i = 0; i < Rlin::dims; ++i )
	{
		Rlin::elem( r, i ) = Alin::elem( a, i ) - Blin::elem( b, i );
	}
	return r;
}

template<
	typename A, typename B,
	typename Alin = linear_traits< A >,
	typename Rlin = typename Alin::template common< B > >
typename Rlin::objtype scale(
	A const& a,
	B const& b,
	typename Alin::elemtype elem = typename Alin::elemtype{} )
{
	typename Rlin::objtype r;
	for( size_t i = 0; i < Rlin::dims; ++i )
	{
		Rlin::elem( r, i ) = Alin::elem( a, i ) * b;
	}
	return r;
}

template<
	typename A, typename B,
	typename Blin = linear_traits< B >,
	typename Rlin = typename Blin::template common< A >,
	typename = typename Blin::elemtype >
typename Rlin::objtype scale(
	A const& a,
	B const& b,
	typename Blin::elemtype elem = typename Blin::elemtype{} )
{
	typename Rlin::objtype r;
	for( size_t i = 0; i < Rlin::dims; ++i )
	{
		Rlin::elem( r, i ) = a * Blin::elem( b, i );
	}
	return r;
}

template<
	typename A, typename B,
	typename Alin = linear_traits< A >,
	typename Rlin = typename Alin::template common< B > >
typename Rlin::objtype invscale(
	A const& a,
	B const& b )
{
	typename Rlin::objtype r;
	for( size_t i = 0; i < Rlin::dims; ++i )
	{
		Rlin::elem( r, i ) = Alin::elem( a, i ) / b;
	}
	return r;
}

template<
	typename A, typename B,
	typename Alin = linear_traits< A >,
	typename Blin = linear_traits< B >,
	typename R = typename Alin::template with< B >::elemtype >
R dot(
	A const& a,
	B const& b )
{
	if( Alin::dims == 0 )
	{
		return R{};
	}
	R r = Alin::elem( a, 0 ) * Blin::elem( b, 0 );
	for( size_t i = 1; i < Alin::dims; ++i )
	{
		r += Alin::elem( a, i ) * Blin::elem( b, i );
	}
	return r;
}

template< typename T, size_t asize, size_t bsize >
matrix< T, bsize, asize > transpose(
	matrix< T, asize, bsize > const& a )
{
	matrix< T, bsize, asize > r;
	for( size_t ai = 0; ai < asize; ++ai )
	{
		for( size_t bi = 0; bi < bsize; ++bi )
		{
			r( bi, ai ) = a( ai, bi );
		}
	}
	return r;
}

template< typename T, typename U, size_t asize, size_t bsize, size_t csize >
matrix< decltype( T{} * U{} ), asize, csize > inner(
	matrix< T, asize, bsize > const& a,
	matrix< U, bsize, csize > const& b )
{
	using V = decltype( T{} * U{} );
	matrix< V, asize, csize > r;
	for( size_t ai = 0; ai < asize; ++ai )
	{
		for( size_t ci = 0; ci < csize; ++ci )
		{
			if( bsize == 0 )
			{
				r( ai, ci ) = V{};
			}
			else
			{
				r( ai, ci ) = a( ai, 0 ) * b( 0, ci );
				for( size_t bi = 1; bi < bsize; ++bi )
				{
					r( ai, ci ) += a( ai, bi ) * b( bi, ci );
				}
			}
		}
	}
	return r;
}

template< typename T, typename U, size_t asize, size_t bsize >
vector< decltype( T{} * U{} ), asize > inner(
	matrix< T, asize, bsize > const& a,
	vector< U, bsize > const& b )
{
	using V = decltype( T{} * U{} );
	vector< V, asize > r;
	for( size_t ai = 0; ai < asize; ++ai )
	{
		if( bsize == 0 )
		{
			r( ai ) = V{};
		}
		else
		{
			r( ai ) = a( ai, 0 ) * b( 0 );
			for( size_t bi = 1; bi < bsize; ++bi )
			{
				r( ai ) += a( ai, bi ) * b( bi );
			}
		}
	}
	return r;
}

template< typename T, typename U, size_t asize, size_t bsize >
vector< decltype( T{} * U{} ), bsize > inner(
	vector< T, asize > const& a,
	matrix< U, asize, bsize > const& b )
{
	using V = decltype( T{} * U{} );
	vector< V, bsize > r;
	for( size_t bi = 0; bi < asize; ++bi )
	{
		if( asize == 0 )
		{
			r( bi ) = V{};
		}
		else
		{
			r( bi ) = a( 0 ) * b( 0, bi );
			for( size_t ai = 1; ai < asize; ++ai )
			{
				r( bi ) += a( ai ) * b( ai, bi );
			}
		}
	}
	return r;
}

template< typename T, typename U, typename V >
auto inner( T const& a, U const& b, V const& c )
	-> decltype( inner( inner( a, b ), c ) )
{
	return inner( inner( a, b ), c );
}

template< typename T, size_t size >
matrix< T, size, size > diag(
	vector< T, size > const& a )
{
	matrix< T, size, size > r = {};
	for( size_t i = 0; i < size; ++i )
	{
		r( i, i ) = a( i );
	}
	return r;
}

template< typename T, size_t size >
matrix< T, size, size > identity()
{
	matrix< T, size, size > r = {};
	for( size_t i = 0; i < size; ++i )
	{
		r( i, i ) = 1;
	}
	return r;
}

template< typename T, typename U >
decltype( T{} * U{} ) det2(
	vector< T, 2 > const& a,
	vector< U, 2 > const& b )
{
	return a( 0 ) * b( 1 ) - a( 1 ) * b( 0 );
}

template< typename T >
vector< T, 2 > cross2(
	vector< T, 2 > const& a )
{
	return { - a( 1 ), a( 0 ) };
}

template< typename A, typename B >
auto operator+=(
	A& a,
	B const& b )
	-> decltype( addassign( a, b ) )
{
	return addassign( a, b );
}

template< typename A, typename B >
auto operator-=(
	A& a,
	B const& b )
	-> decltype( subassign( a, b ) )
{
	return subassign( a, b );
}

template< typename A, typename B >
auto operator*=(
	A& a,
	B const& b )
	-> decltype( scaleassign( a, b ) )
{
	return scaleassign( a, b );
}

template< typename A, typename B >
auto operator/=(
	A& a,
	B const& b )
	-> decltype( invscaleassign( a, b ) )
{
	return invscaleassign( a, b );
}

template< typename A >
auto operator-(
	A const& a )
	-> decltype( invert( a ) )
{
	return invert( a );
}

template< typename A, typename B >
auto operator+(
	A const& a,
	B const& b )
	-> decltype( add( a, b ) )
{
	return add( a, b );
}

template< typename A, typename B >
auto operator-(
	A const& a,
	B const& b )
	-> decltype( sub( a, b ) )
{
	return sub( a, b );
}

template< typename A, typename B >
auto operator*(
	A const& a,
	B const& b )
	-> decltype( scale( a, b ) )
{
	return scale( a, b );
}

template< typename A, typename B >
auto operator/(
	A const& a,
	B const& b )
	-> decltype( invscale( a, b ) )
{
	return invscale( a, b );
}

main.cpp

#include "externals.hpp"
#include "modelfilter.hpp"
#include "controller.hpp"
#include "sim.hpp"
#include "units.hpp"
#include "lin.hpp"
#include <cstdio>
#include <cmath>

namespace modelfilter
{
	extern quantity g;
}

static int32_t time = 0;

static void printstate( FILE* f )
{
	fprintf( f, "%7.3Lf",
		time / unit::s );
	// fprintf( f, ", %22.10Lf",
		// 1.0L * unit::s * unit::s / unit::m *
			// ( long double )modelfilter::g );
	// fprintf( f, ", %22.10Lf, %22.10Lf",
		// ( long double )sim::slope( 0 ),
		// ( long double )sim::slope( 1 ) );
	// fprintf( f, ", %22.10Lf, %22.10Lf",
		// sim::s * ( long double )sim::slopevel( 0 ),
		// sim::s * ( long double )sim::slopevel( 1 ) );
	// fprintf( f, ", %22.10Lf, %22.10Lf",
		// sim::s * sim::s * ( long double )sim::slopeacc( 0 ),
		// sim::s * sim::s * ( long double )sim::slopeacc( 1 ) );
	fprintf( f, ", %22.10Lf, %22.10Lf",
		unit::s / unit::m *
			( long double )modelfilter::ballvel( 0 ),
		unit::s / unit::m *
			( long double )modelfilter::ballvel( 1 ) );
	fprintf( f, ", %22.10Lf, %22.10Lf",
		sim::s / sim::m * ( long double )sim::ballvel( 0 ),
		sim::s / sim::m * ( long double )sim::ballvel( 1 ) );
	fprintf( f, ", %22.10Lf, %22.10Lf",
		1.0L / unit::m *
			( long double )modelfilter::ballpos( 0 ),
		1.0L / unit::m *
			( long double )modelfilter::ballpos( 1 ) );
	fprintf( f, ", %22.10Lf, %22.10Lf",
		1.0L / sim::m * ( long double )sim::ballpos( 0 ),
		1.0L / sim::m * ( long double )sim::ballpos( 1 ) );
	// fprintf( f, ", %22.10Lf, %22.10Lf",
		// ( long double )modelfilter::targetslope( 0 ),
		// ( long double )modelfilter::targetslope( 1 ) );
	// fprintf( f, ", %22.10Lf, %22.10Lf",
		// ( long double )( externals::motorpos + modelfilter::motoroffset )( 0 ),
		// ( long double )( externals::motorpos + modelfilter::motoroffset )( 1 ) );
	fprintf( f, ", %22.10Lf, %22.10Lf",
		( long double )(
			modelfilter::motoroffset( 0 ) - externals::motorposoffset( 0 ) ),
		( long double )(
			modelfilter::motoroffset( 1 ) - externals::motorposoffset( 1 ) ) );
	fprintf( f, "\n" );
	fflush( f );
}

int main()
{
	FILE* f = fopen( "log.csv", "w" );
	if( !f )
	{
		perror( "" );
	}
	printstate( f );
	while( time <= int( 30 * unit::s ) )
	{
		for( int i = 0; i < 10; ++i )
		{
			externals::update();
			modelfilter::update();
			controller::update();
			modelfilter::postupdate();
			time += 1;
		}
		printstate( f );
	}
	fclose( f );
	return 0;
}

modelfilter.cpp

#include "modelfilter.hpp"
#include "externals.hpp"
#include "units.hpp"
#include "lin-form.hpp"
#include "lin.hpp"

namespace modelfilter
{
	quantity const iinvr2 = 1.0_u;
	quantity const gaccfactor = 1.0_u / ( 1.0_u + iinvr2 );
	quantity const cbfactor = iinvr2 / ( 1.0_u + iinvr2 );

	quantity const error_threshold = 0.0001_m2;

	vector< quantity, 2 > motorpostoslope( vector< quantity, 2 > motorpos )
	{
		return motorpos;
	}

	vector< quantity, 2 > slopetomotorpos( vector< quantity, 2 > slope )
	{
		return slope;
	}

	vector< quantity, 2 > targetslope = { 0.0_u, 0.0_u };
	vector< quantity, 2 > ballpos = { 0.0_m, 0.0_m };
	vector< quantity, 2 > ballvel = { 0.0_mps, 0.0_mps };

	vector< quantity, 2 > motoroffset = { 0.0_u, 0.0_u };
	vector< quantity, 2 > slope = { 0.0_u, 0.0_u };
	vector< quantity, 2 > slopevel = { 0.0_ps, 0.0_ps };
	vector< quantity, 2 > slopeacc = { 0.0_ps, 0.0_ps };
	quantity motoroffsetgradfactor = 10000.0_pm2;
	// quantity motoroffsetgradfactor = 10.0_pm2;

	/* state X = { g, vx, vy, qx, qy } */
	quantity g = 9.8_mps2;
	quantity& vx = ballvel( 0 );
	quantity& vy = ballvel( 1 );
	quantity& qx = ballpos( 0 );
	quantity& qy = ballpos( 1 );

	/* state derivatives DX = d/d{a,b} { vx, vy, qx, qy } */
	quantity dx_vx_a = 0.0_u;
	quantity dx_vy_a = 0.0_u;
	quantity dx_qx_a = 0.0_u;
	quantity dx_qy_a = 0.0_u;
	quantity dx_vx_b = 0.0_u;
	quantity dx_vy_b = 0.0_u;
	quantity dx_qx_b = 0.0_u;
	quantity dx_qy_b = 0.0_u;

	/* state covariance matrix P */
	matrix< quantity, 5, 5 > p;

	/* internal noise covariance matrix Q */
	matrix< quantity, 5, 5 > q = diag(
		vector< quantity, 5 >{
			0.0_mps3, 0.1_mps2, 0.1_mps2, 0.1_mps, 0.1_mps } );

	/* measurement matrix H */
	matrix< quantity, 2, 5 > h = {
		0.0_u, 0.0_u, 0.0_u, 1.0_u, 0.0_u,
		0.0_u, 0.0_u, 0.0_u, 0.0_u, 1.0_u };

	int lastballposindex = 0;

	void update()
	{
		/* external parameters */
		vector< quantity, 2 > motorpos = externals::motorpos + motoroffset;
		vector< quantity, 2 > newslope = motorpostoslope( motorpos );
		vector< quantity, 2 > newslopevel = newslope - slope;
		slopeacc = newslopevel - slopevel;
		slopevel = newslopevel;
		slope = newslope;
		quantity qa = slope( 0 );
		quantity qb = slope( 1 );
		quantity va = slopevel( 0 );
		quantity vb = slopevel( 1 );
		quantity aa = slopeacc( 0 );
		quantity ab = slopeacc( 1 );

		quantity qa2 = qa * qa;
		quantity qb2 = qb * qb;
		quantity ca = 1.0_u / ( 1.0_u + qa2 + qb2 );
		quantity cb = ca*ca * cbfactor;
		quantity qaqb = qa * qb;
		quantity ccx = ( 1.0_u + qb2 ) * va - qaqb * vb;
		quantity ccy = ( 1.0_u + qa2 ) * vb - qaqb * va;
		quantity cdx = ccx * cb;
		quantity cdy = ccy * cb;

		/* internal matrix A */
		quantity a_vx_g = - qa * ca * gaccfactor;
		quantity a_vx_vx = - 2.0_u * qa * va * ca;
		quantity a_vx_vy = - 2.0_u * qa * vb * ca;
		quantity a_vx_qx = - ( qa * aa * ca + va * cdx );
		quantity a_vx_qy = - ( qa * ab * ca + vb * cdx );
		quantity a_vy_g = - qb * ca * gaccfactor;
		quantity a_vy_vx = - 2.0_u * qb * va * ca;
		quantity a_vy_vy = - 2.0_u * qb * vb * ca;
		quantity a_vy_qx = - ( qb * aa * ca + va * cdy );
		quantity a_vy_qy = - ( qb * ab * ca + vb * cdy );
		matrix< quantity, 5, 5 > a = {
			0.0_u, 0.0_u, 0.0_u, 0.0_u, 0.0_u,
			a_vx_g, a_vx_vx, a_vx_vy, a_vx_qx, a_vx_qy,
			a_vy_g, a_vy_vx, a_vy_vy, a_vy_qx, a_vy_qy,
			0.0_u, 1.0_u, 0.0_u, 0.0_u, 0.0_u,
			0.0_u, 0.0_u, 1.0_u, 0.0_u, 0.0_u };

		/* advance state X := X + A X dt */
		quantity dvxdt =
			+ a_vx_g * g
			+ a_vx_vx * vx
			+ a_vx_vy * vy
			+ a_vx_qx * qx
			+ a_vx_qy * qy;
		quantity dvydt =
			+ a_vy_g * g
			+ a_vy_vx * vx
			+ a_vy_vy * vy
			+ a_vy_qx * qx
			+ a_vy_qy * qy;
		quantity dqxdt = vx;
		quantity dqydt = vy;
		vx += dvxdt;
		vy += dvydt;
		qx += dqxdt;
		qy += dqydt;

		/* DA = A + I dt */
		a_vx_vx += 1.0_u - 10.0_ps;
		a_vy_vy += 1.0_u - 10.0_ps;
		a( 0, 0 ) += 1.0_u;
		a( 1, 1 ) += 1.0_u - 10.0_ps;
		a( 2, 2 ) += 1.0_u - 10.0_ps;
		a( 3, 3 ) += 1.0_u;
		a( 4, 4 ) += 1.0_u;

		/* advance state derivative DNXN = A DNX + dA/dN X */
		quantity daca = - 2 * qa * ca*ca;
		quantity dacb = 2 * ca * daca * cbfactor;
		quantity daccx = - qb * vb;
		quantity daccy = 2 * qa * vb - qb * va;
		quantity dacdx = daccx * cb + ccx * dacb;
		quantity dacdy = daccy * cb + ccy * dacb;

		quantity dbca = - 2 * qb * ca*ca;
		quantity dbcb = 2 * ca * dbca * cbfactor;
		quantity dbccx = 2 * qb * va - qa * vb;
		quantity dbccy = - qa * va;
		quantity dbcdx = dbccx * cb + ccx * dbcb;
		quantity dbcdy = dbccy * cb + ccy * dbcb;

		quantity dxn_vx_a =
			+ a_vx_vx * dx_vx_a
			+ a_vx_vy * dx_vy_a
			+ a_vx_qx * dx_qx_a
			+ a_vx_qy * dx_qy_a
			- gaccfactor * ( ca + qa * daca ) * g
			- 2 * ( va * ca + qa * va * daca ) * vx
			- 2 * ( vb * ca + qa * vb * daca ) * vy
			- ( aa * ca + qa * aa * daca + va * dacdx ) * qx
			- ( ab * ca + qa * ab * daca + vb * dacdx ) * qy;
		quantity dxn_vy_a =
			+ a_vy_vx * dx_vx_a
			+ a_vy_vy * dx_vy_a
			+ a_vy_qx * dx_qx_a
			+ a_vy_qy * dx_qy_a
			- gaccfactor * qb * daca * g
			- 2 * qb * va * daca * vx
			- 2 * qb * vb * daca * vy
			- ( qb * aa * daca + va * dacdy ) * qx
			- ( qb * ab * daca + vb * dacdy ) * qy;
		quantity dxn_qx_a =
			+ dx_vx_a
			+ dx_qx_a;
		quantity dxn_qy_a =
			+ dx_vy_a
			+ dx_qy_a;
		quantity dxn_vx_b =
			+ a_vx_vx * dx_vx_b
			+ a_vx_vy * dx_vy_b
			+ a_vx_qx * dx_qx_b
			+ a_vx_qy * dx_qy_b
			- gaccfactor * ( qa * dbca ) * g
			- 2 * qa * va * dbca * vx
			- 2 * qa * vb * dbca * vy
			- ( qa * aa * dbca + va * dbcdx ) * qx
			- ( qa * ab * dbca + vb * dbcdx ) * qy;
		quantity dxn_vy_b =
			+ a_vy_vx * dx_vx_b
			+ a_vy_vy * dx_vy_b
			+ a_vy_qx * dx_qx_b
			+ a_vy_qy * dx_qy_b
			- gaccfactor * ( ca + qb * dbca ) * g
			- 2 * ( va * ca + qb * va * dbca ) * vx
			- 2 * ( vb * ca + qb * vb * dbca ) * vy
			- ( aa * ca + qb * aa * dbca + va * dbcdy ) * qx
			- ( ab * ca + qb * ab * dbca + vb * dbcdy ) * qy;
		quantity dxn_qx_b =
			+ dx_vx_b
			+ dx_qx_b;
		quantity dxn_qy_b =
			+ dx_vy_b
			+ dx_qy_b;
		dx_vx_a = dxn_vx_a;
		dx_vy_a = dxn_vy_a;
		dx_qx_a = dxn_qx_a;
		dx_qy_a = dxn_qy_a;
		dx_vx_b = dxn_vx_b;
		dx_vy_b = dxn_vy_b;
		dx_qx_b = dxn_qx_b;
		dx_qy_b = dxn_qy_b;

		/* advance state covariance PN = DA P DA^T + Q dt */
		matrix< quantity, 5, 5 > pn = inner( a, p, transpose( a ) );
		p = pn + q;

		if( externals::ballposindex != lastballposindex )
		{
			lastballposindex = externals::ballposindex;

			/* measurement Z */
			vector< quantity, 2 > z = externals::ballpos;

			/* residual Y */
			vector< quantity, 2 > y = z;
			y( 0 ) -= qx;
			y( 1 ) -= qy;

			/* residual covariance S = H P H^T + R */
			matrix< quantity, 2, 2 > s = inner( h, p, transpose( h ) );

			/* for small errors, adjust gradually */
			/* otherwise, perform a hard correction by setting R = 0 */
			if( dot( y, y ) < error_threshold )
			{
				s += diag(
					vector< quantity, 2 >{
						0.1_m, 0.1_m } );
			}
			else
			{
				dx_vx_a = 0; dx_vx_b = 0;
				dx_vy_a = 0; dx_vy_b = 0;
				dx_qx_a = 0; dx_qx_b = 0;
				dx_qy_a = 0; dx_qy_b = 0;
			}

			quantity sdet = s( 0, 0 ) * s( 1, 1 ) - s( 0, 1 ) * s ( 1, 0 );

			if( sdet < 0.0001_u && sdet > -0.0001_u )
			{
				goto skip;
			}

			matrix< quantity, 2, 2 > invs = {
				s( 1, 1 ), - s( 1, 0 ),
				- s( 0, 1 ), s( 0, 0 ) };
			invs /= sdet;

			/* optimal gain K = P H^T S^-1 */
			matrix< quantity, 5, 2 > k = inner(
				p, transpose( h ), invs );

			/* update state covariance: P := P - K H P */
			matrix< quantity, 5, 5 > pd = inner( k, h, p );
			p -= pd;

			/* update state X := X + K Y */
			g += k( 0, 0 ) * y( 0 ) + k( 0, 1 ) * y( 1 );
			vx += k( 1, 0 ) * y( 0 ) + k( 1, 1 ) * y( 1 );
			vy += k( 2, 0 ) * y( 0 ) + k( 2, 1 ) * y( 1 );
			qx += k( 3, 0 ) * y( 0 ) + k( 3, 1 ) * y( 1 );
			qy += k( 4, 0 ) * y( 0 ) + k( 4, 1 ) * y( 1 );


			if( dot( y, y ) > error_threshold )
			{
				p *= 0;
			}

			/* adjust motor offset */
			quantity grad_a = y( 0 ) * dx_qx_a + y( 1 ) * dx_qy_a;
			quantity grad_b = y( 0 ) * dx_qx_b + y( 1 ) * dx_qy_b;
			motoroffset( 0 ) += motoroffsetgradfactor * grad_a;
			motoroffset( 1 ) += motoroffsetgradfactor * grad_b;
			if( motoroffsetgradfactor > 10.0_pm2 )
			{
				motoroffsetgradfactor *= 1.0_u - 10.0_ps;
			}
			dx_vx_a = 0; dx_vx_b = 0;
			dx_vy_a = 0; dx_vy_b = 0;
			dx_qx_a = 0; dx_qx_b = 0;
			dx_qy_a = 0; dx_qy_b = 0;
		}
skip:
		;
	}

	void postupdate()
	{
		externals::targetmotorpos =
			slopetomotorpos( targetslope ) - motoroffset;
	}
}

modelfilter.hpp

#pragma once

#include "units.hpp"
#include "lin.hpp"

namespace modelfilter
{
	extern vector< quantity, 2 > targetslope;
	extern vector< quantity, 2 > ballpos;
	extern vector< quantity, 2 > ballvel;
	extern vector< quantity, 2 > motoroffset;
	void update();
	void postupdate();
};

sim.cpp

#include "sim.hpp"
#include "common.hpp"

namespace sim
{
	vector< quantity, 2 > motorpostoslope( vector< quantity, 2 > motorpos )
	{
		return motorpos;
	}

	vector< quantity, 2 > ballpos = {
		quantity( -0.2L * m ), quantity( 0.1L * m ) };
	vector< quantity, 2 > ballvel = {
		quantity( 1.0L * m / s ), quantity( 2.0L * m / s ) };
		// quantity( 0.0L * m / s ), quantity( 0.0L * m / s ) };
	vector< quantity, 2 > slope = {
		quantity( 0.0L ), quantity( 0.0L ) };
	vector< quantity, 2 > slopevel = {
		quantity( 0.0L / s ), quantity( 0.0L / s ) };
	vector< quantity, 2 > slopeacc = {
		quantity( 0.0L / s / s ), quantity( 0.0L / s / s ) };
	vector< quantity, 2 > motorpos = motorpostoslope( slope );
	vector< quantity, 2 > motorpos2 = motorpos;
	vector< quantity, 2 > targetmotorpos = motorpos;
	quantity measurementtime = quantity( 0.0L );

	void updatemotor(
		quantity& pos,
		quantity& pos2,
		quantity targetpos )
	{
		pos2 += clip( targetpos - pos2, quantity( -1.0L ), quantity( 1.0L ) )
			* quantity( 1.0L / s );
		pos += ( pos2 - pos ) * quantity( 10.0L / s );
	}

	void step()
	{
		updatemotor(
			motorpos( 0 ), motorpos2( 0 ),
			targetmotorpos( 0 ) );
		updatemotor(
			motorpos( 1 ), motorpos2( 1 ),
			targetmotorpos( 1 ) );

		vector< quantity, 2 > newslope = motorpostoslope( motorpos );
		vector< quantity, 2 > newslopevel = newslope - slope;
		slopeacc = newslopevel - slopevel;
		slopevel = newslopevel;
		slope = newslope;

		quantity oneplusiinvr2 = quantity( 1.0 ) + iinvr2;
		quantity oneplusab2 = quantity( 1.0 ) + dot( slope, slope );
		quantity dirfactor =
			gaccfactor
			+ 2 * dot( ballvel, slopevel )
			+ dot( ballpos, slopeacc );
		dirfactor /= oneplusab2;
		quantity rotfactor =
			iinvr2 * dot( ballpos, slopevel );
		rotfactor /= oneplusiinvr2 * oneplusab2 * oneplusab2;
		vector< quantity, 2 > rotvector =
			slopevel
			+ det2( slope, slopevel )
				* cross2( slope );
		vector< quantity, 2 > ballacc =
			- dirfactor * slope
			- rotfactor * rotvector;
		ballvel += ballacc;
		ballvel *= quantity( 1.0L - 10.0L / s );
		ballpos += ballvel;
	}
}

sim.hpp

#pragma once

#include "lin.hpp"
#include "units.hpp"

namespace sim
{
	using quantity = ::quantity;
	static int timefactor = 10;
	static long double const s = unit::s * timefactor;
	static long double const m = 1.0L;
	static quantity const g = quantity( 9.8L * m / s / s );
	static quantity const iinvr2 = quantity( 1.0 );
	static quantity const gaccfactor = g / ( quantity( 1.0 ) + iinvr2 );
	static quantity const measurementinterval = quantity( unit::s / 30.0L );

	extern vector< quantity, 2 > ballpos;
	extern vector< quantity, 2 > ballvel;
	extern vector< quantity, 2 > slope;
	extern vector< quantity, 2 > slopevel;
	extern vector< quantity, 2 > slopeacc;
	extern vector< quantity, 2 > motorpos;
	extern vector< quantity, 2 > motorpos2;
	extern vector< quantity, 2 > targetmotorpos;
	extern quantity measurementtime;

	void step();
}

units.hpp

#pragma once

using quantity = float;

namespace unit
{
	static long double const s = 1000.0L;
	static long double const m = 1000.0L;
}

inline quantity operator"" _u( long double value )
{
	return quantity( value );
}

inline quantity operator"" _s( long double value )
{
	return quantity( unit::s * value );
}

inline quantity operator"" _ps( long double value )
{
	return quantity( 1.0L / unit::s * value );
}

inline quantity operator"" _ps2( long double value )
{
	return quantity( 1.0L / unit::s / unit::s * value );
}

inline quantity operator"" _m( long double value )
{
	return quantity( unit::m * value );
}

inline quantity operator"" _pm( long double value )
{
	return quantity( 1.0L / unit::m * value );
}

inline quantity operator"" _pm2( long double value )
{
	return quantity( 1.0L / unit::m / unit::m * value );
}

inline quantity operator"" _m2( long double value )
{
	return quantity( unit::m * unit::m * value );
}

inline quantity operator"" _mps( long double value )
{
	return quantity( unit::m / unit::s * value );
}

inline quantity operator"" _mps2( long double value )
{
	return quantity( unit::m / unit::s / unit::s * value );
}

inline quantity operator"" _mps3( long double value )
{
	return quantity( unit::m / unit::s / unit::s / unit::s * value );
}

inline quantity operator"" _m2ps2( long double value )
{
	return quantity( unit::m * unit::m / unit::s / unit::s * value );
}

inline quantity operator"" _spm( long double value )
{
	return quantity( unit::s / unit::m * value );
}