Created
July 15, 2018 00:30
-
-
Save delfigamer/e95ed281a825db8df691f85493c58b1c to your computer and use it in GitHub Desktop.
Control system with object simulation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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; | |
| } | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #pragma once | |
| #include "units.hpp" | |
| #include "lin.hpp" | |
| namespace controller | |
| { | |
| extern vector< quantity, 2 > targetpos; | |
| void update(); | |
| }; |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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; | |
| } | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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(); | |
| }; |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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 ); | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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; | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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; | |
| } | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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(); | |
| }; |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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; | |
| } | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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(); | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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