199 lines
5.2 KiB
C++
199 lines
5.2 KiB
C++
/*
|
||
* ForceEstimation.cpp
|
||
*
|
||
* Created on: 4 апр. 2022 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#include "ForceEstimation.hh"
|
||
|
||
processing::ForceEstimation::ForceEstimation(systemic::ISignal & speed,
|
||
systemic::ISignal & force,
|
||
systemic::ISignal & position ) :
|
||
speed(speed), force(force), position(position), est() {}
|
||
|
||
const float & processing::ForceEstimation::getEstimatedForce() const {
|
||
|
||
return force_est;
|
||
|
||
}
|
||
|
||
const float & processing::ForceEstimation::getEstimatedForceRate() const {
|
||
|
||
return force_rate_est;
|
||
|
||
}
|
||
|
||
const float & processing::ForceEstimation::getEstimatedKhardness() const {
|
||
|
||
return est.khardness_est;
|
||
|
||
}
|
||
|
||
vector::ITechValue & processing::ForceEstimation::getKhardness() {
|
||
|
||
return est;
|
||
|
||
}
|
||
|
||
void processing::ForceEstimation::configure( Setting & config ) {
|
||
|
||
delay = config.force_delay;
|
||
min_force = config.min_force;
|
||
|
||
est.L1N = config.L1N;
|
||
est.L2N = config.L2N;
|
||
|
||
est.L1P = config.L1P;
|
||
est.L2P = config.L2P;
|
||
|
||
est.permit_error = config.permit_error;
|
||
|
||
est.transmission_ratio = config.transmission_ratio;
|
||
est._1_transmission_ratio = 1.0f / config.transmission_ratio;
|
||
|
||
est.set( config.khardness );
|
||
|
||
const float minimal_force_delta = 2.0f;
|
||
est.dead_displacement = minimal_force_delta / ( config.khardness * config.transmission_ratio );
|
||
|
||
}
|
||
|
||
void processing::ForceEstimation::process() {
|
||
|
||
using namespace std;
|
||
|
||
float force_value = force;
|
||
|
||
if( isfinite(force_value) ) {
|
||
|
||
if( force_value > min_force ) {
|
||
|
||
if( force_prev != force_value )
|
||
est.estimate( position, force_value );
|
||
|
||
force_est = est.predict( position + speed * delay );
|
||
|
||
|
||
force_prev = force_value;
|
||
|
||
} else {
|
||
|
||
force_prev = 0.0f;
|
||
force_est = 0.0f;
|
||
|
||
est.force_est = min_force;
|
||
est.position_est = position;
|
||
|
||
est.prev_displacement = 0.0f;
|
||
est.prev_force_error = 0.0f;
|
||
|
||
}
|
||
|
||
}
|
||
|
||
force_rate_est = speed * est.khardness_est;
|
||
|
||
}
|
||
|
||
processing::ForceEstimation::Setting::Setting() :
|
||
min_force(-INFINITY), khardness(-INFINITY), force_delay(-INFINITY), transmission_ratio(0.0f),
|
||
L1P(-INFINITY), L2P(-INFINITY), L1N(-INFINITY), L2N(-INFINITY), permit_error(-INFINITY) {}
|
||
|
||
bool processing::ForceEstimation::Setting::isValid() {
|
||
using namespace std;
|
||
|
||
using namespace std;
|
||
|
||
const Setting non_valid;
|
||
|
||
if( min_force < 0.0f or not isfinite(min_force) )
|
||
min_force = non_valid.min_force;
|
||
|
||
if( khardness < 0.0f or not isfinite(khardness) )
|
||
khardness = non_valid.khardness;
|
||
|
||
if( force_delay < 0.0f or not isfinite(force_delay) )
|
||
force_delay = non_valid.force_delay;
|
||
|
||
if( transmission_ratio <= 0.0f or not isfinite(transmission_ratio) )
|
||
transmission_ratio = non_valid.transmission_ratio;
|
||
|
||
if( L1P < 0.0f or not isfinite(L1P) )
|
||
L1P = non_valid.L1P;
|
||
|
||
if( L2P < 0.0f or not isfinite(L2P) )
|
||
L2P = non_valid.L2P;
|
||
|
||
if( L1N < 0.0f or not isfinite(L1N) )
|
||
L1P = non_valid.L1N;
|
||
|
||
if( L2N < 0.0f or not isfinite(L2N) )
|
||
L2N = non_valid.L2N;
|
||
|
||
if( permit_error <= 0.0f or not isfinite(permit_error) )
|
||
permit_error = non_valid.permit_error;
|
||
|
||
return min_force != non_valid.min_force and
|
||
khardness != non_valid.khardness and
|
||
force_delay != non_valid.force_delay and
|
||
transmission_ratio != non_valid.transmission_ratio and
|
||
L1P != non_valid.L1P and
|
||
L2P != non_valid.L2P and
|
||
L1N != non_valid.L1N and
|
||
L2N != non_valid.L2N and
|
||
permit_error != non_valid.permit_error;
|
||
|
||
}
|
||
|
||
void processing::ForceEstimation::setSampleTime( float ts_in_second ) {
|
||
|
||
_Ts = ts_in_second;
|
||
|
||
}
|
||
|
||
void processing::ForceEstimation::Estimator::estimate(float value_position,
|
||
float value_force) {
|
||
|
||
using namespace std;
|
||
|
||
float displacement = value_position - position_est;
|
||
float force_perdicted = force_est + khardness_est * displacement;
|
||
float force_error = value_force - force_perdicted;
|
||
|
||
float L1 = ( displacement > 0.0f ? L1P : L1N );
|
||
float L2 = fabsf(displacement) > dead_displacement ? ( displacement > 0.0f ? L2P : L2N ) : 0.0f;
|
||
|
||
//Update logic
|
||
if( fabsf(force_error) < permit_error and
|
||
fabsf(displacement) < intolerable_displacement ) {
|
||
|
||
khardness_est = khardness_est + force_error * L2;
|
||
force_est = force_perdicted + force_error * L1;
|
||
position_est = value_position;
|
||
|
||
}
|
||
|
||
//Reset logic on big-error
|
||
if( ( fabsf(displacement) >= intolerable_displacement
|
||
and fabsf(prev_displacement) >= intolerable_displacement ) or
|
||
( fabsf(force_error) >= permit_error
|
||
and fabsf(prev_force_error) >= permit_error ) ) {
|
||
|
||
force_est = value_force;
|
||
position_est = value_position;
|
||
|
||
}
|
||
|
||
prev_displacement = displacement;
|
||
prev_force_error = force_error ;
|
||
|
||
}
|
||
|
||
float processing::ForceEstimation::Estimator::predict(float value_position) {
|
||
|
||
float displacement = value_position - position_est;
|
||
return force_est + displacement * khardness_est;
|
||
|
||
}
|