Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
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
You can’t perform that action at this time.