Open phillipstanleymarbell opened 4 years ago
We need an intuitive way to represent uncertainty in expressions at the notation of language. Below are some example choices for the notation. The units are arbitrary choices for an undescribed system with state units (kg, m, jf)
.
# Using the common in literature tilde
x1 ~ Gaussian (mean: 3 kg, var: 0.1 kg*kg)
# or without "named arguments" [*]
x1 ~ Gaussian(3 kg, 0.1 kg*kg)
----------
# Value after the units (could be with tilde, too)
x1 = 3 Kg 0.1
----------
# Shorter, unit "outside" the distribution
x1 = N(3, 0.1) kg
*[]** This approach is verbose but looks preferable. It is used both in the AutoFilter and the Stan programming language.
Another interesting to also address together would be denoting the covariance between random variables or expressions. This is important, for example, in state estimation, where there is a covariance matrix in the calculations for the covariance between the state variables. A notation for this could be something like:
cov(x1, x2) = 0.01 kg*m
cov(x1, x3) = 0.01 kg*jf
cov(x2, x3) = 0.01 m*jf
For 3 state variables its 3 statements. Of course this is bound to get cumbersome with more state variables, (N choose 2) statements for N random variables. This is usually best represented by a covariance matrix, though it uses N*N "statements". So with 3 state variables its 9. Here is what it could look like in syntax:
covmatrix = (( 0.1 kg*kg, 0.01 kg*m, 0.01 kg*jf),
( 0.01 m*kg, 0.1 m*m, 0.01 m*jf),
(0.01 jf*kg, 0.01 jf*m, 0.1 jf*jf))
Notice that this also includes the variances in the diagonal. The (N choose 2) statements needed instead of (N+1 choose 2) in the cov(a,b)
method is because the variances are already at the declaration of the distribution.
A sketchout of the API functions of the generated code for the #463. The dynamic system in the example has 9-dimensional state, 3 for each of Distance, Velocity and Acceleration.
It would be a good idea to be able to provide a prefix, such as filter-
below, so that multiple filters can coexist in the same codebase.
filterCoreStateIdx
: An enumerator to be used in indexing.CoreState
: Contains (at least) the current state and the current state covariance as arrays.filterInit()
: Used to initialize the CoreState
.filterUpdate()
: Incorporates new measurement to the state. Adjusts gain and state covariance accordingly.filterPredict()
: Propagates the state and the state covariance by time step through the system dynamics.filterCheck()
: Evaluates the likelihood of a given state vector representing the system's state.An example is illustrated below:
typedef enum
{
STATE_multiAxisDistance1,
STATE_multiAxisDistance2,
STATE_multiAxisDistance3,
STATE_multiAxisSpeed1,
STATE_multiAxisSpeed2,
STATE_multiAxisSpeed3,
STATE_multiAxisAcceleration1,
STATE_multiAxisAcceleration2,
STATE_multiAxisAcceleration3,
STATE_DIMENSION
} filterCoreStateIdx;
typedef struct {
/*
* State
*/
double S[STATE_DIMENSION];
/*
* State covariance matrix
*/
double P[STATE_DIMENSION][STATE_DIMENSION];
} CoreState;
void
filterInit (double S[STATE_DIMENSION], double P[STATE_DIMENSION][STATE_DIMENSION]);
void
filterUpdate (filterCoreStateIdx state_no, double measured_value);
/*
* Propagate the core state of the filter through time.
*
* step: the time by which to forward state
*/
void
filterPredict (double step);
/*
* A likelihood function to check the probability of a given vector
* representing the system's state.
*/
double
filterCheck (double S[STATE_DIMENSION]);
Here are example newton descriptions. One is just a sample one, generated from mperl, the other two are manually written ones which parse fine, but don't yield the expected result / don't report back the same result when substituting parameters and signals.
test_torque.nt.txt test_flexural.nt.txt mperl_generated.nt.txt
This is not yet the full picture but just the generation of the pi groups that are part of the full model. @marcuspirron is merging the expected result with the description of the scara arm to have the full model. That will be the complete example for the EKF backend.
Let's talk tomorrow about it. Attached is an updated version of the mperl generated newton description.
An example for the notation (though not correct physically): Roomba-estSynth.nt.txt
Here is the update version of the scara arm. I cannot push to the repo so I'm just pasting it below.
@KomaGR: for the measurements, we were not sure if it is possible to handle indirect measurements. For instance, a line 70, we would like to measure the force indirectly using the weight of an object. If we don't declare the weight the parser complains that it is not declared. But if we declare it, we have to provide a value for it, right?
The file containts 2 models: a rigid body and one with beam deflexion (Euler Bernoulli model).
For the 2nd model, we included a few possible set of measurements.
The values of some constants may be off and lines 107-108 are probably wrong but at least they typecheck.
include "NewtonBaseSignals.nt"
# Additional includes and signals
electricResistance : signal = {
name = "Ohm" English;
symbol = O;
derivation = power/current;
}
electricPotential : signal = {
name = "Electric Potential" English;
symbol = V;
derivation = work / charge;
}
torque: signal = {
name = "Torque" English;
symbol = t;
derivation = angularDisplacement * distance * force;
}
secondMomentOfArea : signal = {
name = "Second Moment of area" English;
symbol = I;
derivation = distance * distance * distance * distance;
}
flexuralModulus : signal = {
name = "Flexural Modulus" English;
symbol = E;
derivation = force * distance * distance * distance / (secondMomentOfArea * distance);
}
# Model of mperl model:
# In this section, the complete description of the mperl generated
# model is given.
# length of the beams
Prismatic0_length: constant = 0.2 m;
Prismatic1_length: constant = 0.2 m;
# x0, y0, z0: Origin (y is up)
x0: constant = 0 m;
y0: constant = 0 m;
z0: constant = 0 m;
#################
#################
#################
# Rigid body model
endeffector_1: invariant(
R0_angle: angularDisplacement, # set point of the actuators
R1_angle: angularDisplacement,
x: distance, # position
y: distance,
z: distance,
mForce: force, # force pulling down
F: torque # torque on the motor at the base
) =
{
x ~ x0 + Prismatic0_length * cos(R0_angle) + Prismatic1_length * cos(R0_angle+R1_angle),
y ~ y0 + Prismatic0_length * sin(R0_angle) + Prismatic1_length * sin(R0_angle+R1_angle),
z ~ z0,
F ~ ((x-x0)**2 + (y-y0)**2 + (z-z0)**2)**(1/2) * mForce * ( kNewtonUnitfree_pi/2-arctan(y/x) )
}
endeffector_1_measure: invariant(
R0_angle: angularDisplacement,
R1_angle: angularDisplacement,
mWeight: kilogram # the mass pulling down
) =
{
mForce ~ kNewtonUnithave_AccelerationDueToGravity * mWeight
}
#################
#################
#################
flexMod : constant = 1900000 (N * m **-2);
areaMoment : constant = (1/12 * 0.005 * 0.05 ** 3) (m ** 4);
# Euler beam model
endeffector_2: invariant(
R0_angle: angularDisplacement, # set point of the actuators
R1_angle: angularDisplacement,
R0_effective_angle: angularDisplacement, # overall angle including a small deflection
R1_effective_angle: angularDisplacement,
x: distance, # position
y: distance,
z: distance,
mForce: force, # force pulling down at the effector
F0: torque, # torque on the motor at R0
F1: torque # torque on the motor at R1
) =
{
R0_effective_angle ~ R0_angle + mForce * cos(R0_angle) * Prismatic0_length ** 2 / (3*flexMod*areaMoment), #FIXME probably wrong
R1_effective_angle ~ R1_angle + mForce * cos(R0_effective_angle + R1_angle) * Prismatic1_length ** 2 / (3*flexMod*areaMoment), #FIXME probably wrong
x ~ x0 + Prismatic0_length*cos(R0_effective_angle) + Prismatic1_length*cos(R0_effective_angle + R1_effective_angle),
y ~ y0 + Prismatic0_length*sin(R0_effective_angle) + Prismatic1_length*sin(R0_effective_angle + R1_effective_angle),
z ~ z0,
F0 ~ ((x-x0)**2 + (y-y0)**2 + (z-z0)**2)**(1/2) * mForce * ( kNewtonUnitfree_pi/2-arctan(y/x) ),
F1 ~ Prismatic1_length * mForce * sin(R0_effective_angle + R1_effective_angle)
}
# no embedded sensor to measure deflexion
endeffector_2_measure_1: invariant(
R0_angle: angularDisplacement,
R1_angle: angularDisplacement,
x: distance,
y: distance,
z: distance,
mWeight: kilogram # the mass pulling down
) =
{
mForce ~ kNewtonUnithave_AccelerationDueToGravity * mWeight
}
# with embedded flex sensors returning angles
endeffector_2_measure_2: invariant(
R0_angle: angularDisplacement,
R1_angle: angularDisplacement,
Beam0Deflexion: angularDisplacement,
Beam1Deflexion: angularDisplacement,
mWeight: kilogram # the mass pulling down
) =
{
mForce ~ kNewtonUnithave_AccelerationDueToGravity * mWeight,
R0_effective_angle ~ R0_angle + Beam0Deflexion,
R1_effective_angle ~ R1_angle + Beam1Deflexion
}
# with embedded flex sensors returning voltage
Flex_sensor_resistance_delta: constant = (11000 / 3.14159 * 4) (O * rad ** (-1));
Flex_sensor_resistance_base: constant = 17000 O;
vRef: constant = 5 V;
bridge_r1: constant = 1000 O;
bridge_r2: constant = 1000 O;
bridge_r3: constant = 1000 O;
endeffector_2_measure_3: invariant(
R0_angle: angularDisplacement,
R1_angle: angularDisplacement,
Flex0: electricPotential,
Flex1: electricPotential,
mWeight: kilogram
) =
{
mForce ~ kNewtonUnithave_AccelerationDueToGravity * mWeight,
R0_effective_angle ~ R0_angle + ( (bridge_r2 * vRef - (bridge_r1 + bridge_r2) * Flex0) /
(bridge_r1 * vRef + (bridge_r1 + bridge_r2) * Flex0) * bridge_r3
- Flex_sensor_resistance_base ) / Flex_sensor_resistance_delta,
R1_effective_angle ~ R1_angle + ( (bridge_r2 * vRef - (bridge_r1 + bridge_r2) * Flex1) /
(bridge_r1 * vRef + (bridge_r1 + bridge_r2) * Flex1) * bridge_r3
- Flex_sensor_resistance_base ) / Flex_sensor_resistance_delta
}
# measuring torque through motor current
torquePerAmp: constant = 1 (t * rad ** (-1));
endeffector_2_measure_4: invariant(
C0: current,
C1: current,
mWeight: kilogram
) =
{
mForce ~ kNewtonUnithave_AccelerationDueToGravity * mWeight,
F0 ~ C0 * torquePerAmp,
F1 ~ C1 * torquePerAmp
}
For instance, a line 70, we would like to measure the force indirectly using the weight of an object. If we don't declare the weight the parser complains that it is not declared. But if we declare it, we have to provide a value for it, right?
Yes, that is correct. In the measurement invariant constraints you are declaring all that you can measure in the left-hand sides and in their right-hand sides, how these values may also be computed from the state. I've come to understand that this means the inverse of the function used to get the state from the sensors. These things mean that the measurement invariant constraints should look something like this:
{
sensor_1 ~ h_1(state_1, state_2, ..., state_n),
sensor_2 ~ h_2(state_1, state_2, ..., state_n),
...
sensor_z ~ h_z(state_1, state_2, ..., state_n)
}
Apropos, the process invariant constraints should look something like this:
{
state_1 ~ f_1(state_1, state_2, ..., state_n),
state_2 ~ f_2(state_1, state_2, ..., state_n),
...
state_n ~ f_z(state_1, state_2, ..., state_n)
}
I think this will be a bit strange in this use-case given that, as I've understood, it is a static (or mostly static) process. What this means is, even though you may have other invariants describing the system, the process invariant constraint might look like this (essentially relating the value at different timestamps) for any states that are static:
{
state_1 ~ state_1
state_2 ~ state_2
...
state_n ~ state_n
}
The file containts 2 models: a rigid body and one with beam deflexion (Euler Bernoulli model). For the 2nd model, we included a few possible set of measurements.
Since choices for compilation are cli-driven, you should be able to choose which 2 invariants to compile each time regardless of how many there are in the Newton description file.
Right now the model is mostly static constraints.
To make it a bit better, we can make the motor position vary gradually something with:
...
R0_set_point: angularDisplacement, # input from the controller
R0_angle: angularDisplacement, # state of the actuators
...
{
R0_set_point ~ R0_set_point
R0_angle ~ R0_angle + 0.1 * (R0_set_point - R0_angle)
...
Most measurement functions are easy to invert. endeffector_2_measure_3
is harder but the process is still doable. I'll think about that.
Below is a new version. I re-inverted the measurements and added dt
and a proportional controller so that not everything is static.
include "NewtonBaseSignals.nt"
# Additional includes and signals
electricResistance : signal = {
name = "Ohm" English;
symbol = O;
derivation = power/current;
}
electricPotential : signal = {
name = "Electric Potential" English;
symbol = V;
derivation = work / charge;
}
torque: signal = {
name = "Torque" English;
symbol = t;
derivation = angularDisplacement * distance * force;
}
secondMomentOfArea : signal = {
name = "Second Moment of area" English;
symbol = I;
derivation = distance * distance * distance * distance;
}
flexuralModulus : signal = {
name = "Flexural Modulus" English;
symbol = E;
derivation = force * distance * distance * distance / (secondMomentOfArea * distance);
}
# Model of mperl model:
# In this section, the complete description of the mperl generated
# model is given.
# length of the beams
Prismatic0_length: constant = 0.2 m;
Prismatic1_length: constant = 0.2 m;
# x0, y0, z0: Origin (y is up)
x0: constant = 0 m;
y0: constant = 0 m;
z0: constant = 0 m;
# controller propostionality constant
pCtr: constant = 0.2 (s ** (-1));
#################
#################
#################
# Rigid body model
endeffector_1: invariant(
R0_set_point: angularDisplacement, # set point of the actuators
R1_set_point: angularDisplacement,
R0_angle: angularDisplacement, # state of the actuators
R1_angle: angularDisplacement,
x: distance, # position of the end effector
y: distance,
z: distance,
mForce: force, # force pulling down
F: torque, # torque on the motor at the base
dt : time
) =
{
R0_set_point ~ R0_set_point,
R1_set_point ~ R1_set_point,
R0_angle ~ R0_angle + pCtr * (R0_set_point - R0_angle) * dt, # simple proportional controller
R1_angle ~ R1_angle + pCtr * (R1_set_point - R1_angle) * dt, # simple proportional controller
x ~ x0 + Prismatic0_length * cos(R0_angle) + Prismatic1_length * cos(R0_angle+R1_angle),
y ~ y0 + Prismatic0_length * sin(R0_angle) + Prismatic1_length * sin(R0_angle+R1_angle),
z ~ z0 ,
F ~ ((x-x0)**2 + (y-y0)**2 + (z-z0)**2)**(1/2) * mForce * ( kNewtonUnitfree_pi/2-arctan(y/x) )
}
endeffector_1_measure: invariant(
R0_set_point: angularDisplacement,
R1_set_point: angularDisplacement,
mWeight: kilogram, # the mass pulling down
dt: time
) =
{
dt ~ dt,
R0_set_point ~ R0_set_point,
R1_set_point ~ R1_set_point,
mWeight ~ mForce / kNewtonUnithave_AccelerationDueToGravity
}
#################
#################
#################
flexMod : constant = 1900000 (N * m **-2);
areaMoment : constant = (1/12 * 0.005 * 0.05 ** 3) (m ** 4);
# Euler beam model
endeffector_2: invariant(
R0_set_point: angularDisplacement, # set point of the actuators
R1_set_point: angularDisplacement,
R0_angle: angularDisplacement, # state of the actuators
R1_angle: angularDisplacement,
R0_effective_angle: angularDisplacement, # overall angle including a small deflection
R1_effective_angle: angularDisplacement,
x: distance, # position
y: distance,
z: distance,
mForce: force, # force pulling down at the effector
F0: torque, # torque on the motor at R0
F1: torque, # torque on the motor at R1
dt : time
) =
{
R0_set_point ~ R0_set_point,
R1_set_point ~ R1_set_point,
R0_angle ~ R0_angle + pCtr * (R0_set_point - R0_angle) * dt, # simple proportional controller
R1_angle ~ R1_angle + pCtr * (R1_set_point - R1_angle) * dt, # simple proportional controller
R0_effective_angle ~ R0_angle + mForce * cos(R0_angle) * Prismatic0_length ** 2 / (3*flexMod*areaMoment), #FIXME probably wrong
R1_effective_angle ~ R1_angle + mForce * cos(R0_effective_angle + R1_angle) * Prismatic1_length ** 2 / (3*flexMod*areaMoment), #FIXME probably wrong
x ~ x0 + Prismatic0_length*cos(R0_effective_angle) + Prismatic1_length*cos(R0_effective_angle + R1_effective_angle),
y ~ y0 + Prismatic0_length*sin(R0_effective_angle) + Prismatic1_length*sin(R0_effective_angle + R1_effective_angle),
z ~ z0,
F0 ~ ((x-x0)**2 + (y-y0)**2 + (z-z0)**2)**(1/2) * mForce * ( kNewtonUnitfree_pi/2-arctan(y/x) ),
F1 ~ Prismatic1_length * mForce * sin(R0_effective_angle + R1_effective_angle)
}
# no embedded sensor to measure deflexion
endeffector_2_measure_1: invariant(
R0_set_point: angularDisplacement,
R1_set_point: angularDisplacement,
x: distance,
y: distance,
z: distance,
mWeight: kilogram, # the mass pulling down
dt : time
) =
{
dt ~ dt,
R0_set_point ~ R0_set_point,
R1_set_point ~ R1_set_point,
mWeight ~ mForce / kNewtonUnithave_AccelerationDueToGravity
}
# with embedded flex sensors returning angles
endeffector_2_measure_2: invariant(
R0_set_point: angularDisplacement,
R1_set_point: angularDisplacement,
Beam0Deflexion: angularDisplacement,
Beam1Deflexion: angularDisplacement,
mWeight: kilogram, # the mass pulling down
dt : time
) =
{
dt ~ dt,
R0_set_point ~ R0_set_point,
R1_set_point ~ R1_set_point,
mWeight ~ mForce / kNewtonUnithave_AccelerationDueToGravity,
mForce ~ kNewtonUnithave_AccelerationDueToGravity * mWeight,
Beam0Deflexion ~ R0_effective_angle - R0_angle,
Beam1Deflexion ~ R1_effective_angle - R1_angle
}
# with embedded flex sensors returning voltage
Flex_sensor_resistance_delta: constant = (11000 / 3.14159 * 4) (O * rad ** (-1));
Flex_sensor_resistance_base: constant = 17000 O;
vRef: constant = 5 V;
bridge_r1: constant = 1000 O;
bridge_r2: constant = 1000 O;
bridge_r3: constant = 1000 O;
endeffector_2_measure_3: invariant(
R0_set_point: angularDisplacement,
R1_set_point: angularDisplacement,
Flex0: electricPotential,
Flex1: electricPotential,
mWeight: kilogram,
dt: time
) =
{
dt ~ dt,
R0_set_point ~ R0_set_point,
R1_set_point ~ R1_set_point,
mWeight ~ mForce / kNewtonUnithave_AccelerationDueToGravity,
Flex0 ~ vRef * (
bridge_r2 / (bridge_r1 + bridge_r2)
- ((R0_effective_angle - R0_angle) * Flex_sensor_resistance_delta + Flex_sensor_resistance_base) / (bridge_r2 + bridge_r3)
), # measure through a Wheatstone bridge
Flex1 ~ vRef * (
bridge_r2 / (bridge_r1 + bridge_r2)
- ((R1_effective_angle - R1_angle) * Flex_sensor_resistance_delta + Flex_sensor_resistance_base) / (bridge_r2 + bridge_r3)
) # measure through a Wheatstone bridge
}
# measuring torque through motor current
torquePerAmp: constant = 1 (t * rad ** (-1)); #TODO get number from datasheet
endeffector_2_measure_4: invariant(
R0_set_point: angularDisplacement,
R1_set_point: angularDisplacement,
C0: current,
C1: current,
mWeight: kilogram,
dt: time
) =
{
dt ~ dt,
R0_set_point ~ R0_set_point,
R1_set_point ~ R1_set_point,
mForce ~ kNewtonUnithave_AccelerationDueToGravity * mWeight,
C0 ~ F0 / torquePerAmp,
C1 ~ F1 / torquePerAmp
}
The estimator synthesis backend can be invoked like this for the ScaraArm.nt:
./newton-linux-EN --estimator-synthesis=scaraArmApi.c --auto-diff
--process=endeffector_2 --measurement=endeffector_2_measure_3
../../applications/newton/invariants/ScaraArm.nt
The backend lives in issue-463
and is tracked in #463, so make sure this is the checked out branch. The --auto-diff
argument is optional and flags the compiler to generate with Automatic Differentiation.
train()
call into library to feed in samples relating to signals in Newton descriptioncheck()
call to provide set of signal names and get a value back indicating if invariants involving them are violated or if one single is left free, value range (or distribution) it should take to ensure invariant validity