Control system with object simulation
#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; | |
} | |
} |
#pragma once | |
#include "units.hpp" | |
#include "lin.hpp" | |
namespace controller | |
{ | |
extern vector< quantity, 2 > targetpos; | |
void update(); | |
}; |
#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; | |
} | |
} |
#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(); | |
}; |
#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 ); | |
} |
#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; | |
} |
#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; | |
} | |
} |
#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(); | |
}; |
#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; | |
} | |
} |
#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(); | |
} |
#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 ); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment