/**
* If either the <i>impulse</i> or <i>initialState</i> input is unknown, then return false.
* Otherwise, return true.
*
* @return True If the actor is ready to fire.
* @throws IllegalActionException If the superclass throws it.
*/
public boolean prefire() throws IllegalActionException {
boolean result = super.prefire();
if ((impulse.getWidth() == 0 || impulse.isKnown(0))
&& (initialState.getPort().getWidth() == 0 || initialState.getPort().isKnown(0))) {
return result;
}
return false;
}
/**
* Construct an actor with the given container and name.
*
* @param container The container.
* @param name The name of this actor.
* @exception IllegalActionException If the actor cannot be contained by the proposed container.
* @exception NameDuplicationException If the container already has an actor with this name.
*/
public Gaussian(CompositeEntity container, String name)
throws NameDuplicationException, IllegalActionException {
super(container, name);
output.setTypeEquals(BaseType.DOUBLE);
mean = new PortParameter(this, "mean", new DoubleToken(0.0));
mean.setTypeEquals(BaseType.DOUBLE);
new Parameter(mean.getPort(), "_showName", BooleanToken.TRUE);
standardDeviation = new PortParameter(this, "standardDeviation");
standardDeviation.setExpression("1.0");
standardDeviation.setTypeEquals(BaseType.DOUBLE);
new Parameter(standardDeviation.getPort(), "_showName", BooleanToken.TRUE);
}
/**
* Initialize particle filter parameters.
*
* @exception IllegalActionException
* @exception NameDuplicationException
*/
private void _init() throws IllegalActionException, NameDuplicationException {
StringToken[] stateNames = new StringToken[2];
stateNames[0] = new StringToken("x");
stateNames[1] = new StringToken("y");
stateVariableNames.setToken(new ArrayToken(BaseType.STRING, stateNames));
stateVariableNames.setVisibility(Settable.EXPERT);
observerPosition = new PortParameter(this, "observerPosition");
observerPosition.setExpression("{0.0,0.0}");
SingletonParameter showName =
(SingletonParameter) observerPosition.getPort().getAttribute("_showName");
if (showName == null) {
showName = new SingletonParameter(observerPosition.getPort(), "_showName");
showName.setToken("true");
} else {
showName.setToken("true");
}
// The input port for range measurements.
z_m = new TypedIOPort(this, "z_m", true, false);
z_m.setTypeEquals(BaseType.DOUBLE);
showName = (SingletonParameter) z_m.getAttribute("_showName");
z_m.setDisplayName("rangeMeasurement");
if (showName == null) {
showName = new SingletonParameter(z_m, "_showName");
showName.setToken("true");
} else {
showName.setToken("true");
}
// The parameter that contains the measurement expression.
z = new Parameter(this, "z");
z.setExpression("sqrt((x-observerPosition(0))^2 + (y-observerPosition(1))^2)");
z.setVisibility(Settable.EXPERT);
x_update = new Parameter(this, "x_update");
x_update.setExpression("x");
y_update = new Parameter(this, "y_update");
y_update.setExpression("y");
measurementCovariance.setExpression("[2.0]");
prior.setExpression("{random()*200-100,random()*200-100}");
processNoise.setExpression("multivariateGaussian({0.0,0.0},[3.0,0.0;0.0,3.0])");
particleCount.setExpression("2000");
bootstrap.setVisibility(Settable.EXPERT);
lowVarianceSampler.setVisibility(Settable.EXPERT);
}
/**
* If the value at the <i>derivative</i> port is known, and the current step size is bigger than
* 0, perform an integration. If the <i>impulse</i> port is known and has data, then add the value
* provided to the state; if the <i>initialState</i> port is known and has data, then reset the
* state to the provided value. If both <i>impulse</i> and <i>initialState</i> have data, then
* <i>initialState</i> dominates. If either is unknown, then simply return, leaving the output
* unknown. Note that the signals provided at these two ports are required to be purely discrete.
* This is enforced by throwing an exception if the current microstep is zero when they have input
* data.
*
* @exception IllegalActionException If the input is infinite or not a number, or if thrown by the
* solver, or if data is present at either <i>impulse</i> or <i>initialState</i> and the step
* size is greater than zero.
*/
public void fire() throws IllegalActionException {
ContinuousDirector dir = (ContinuousDirector) getDirector();
double stepSize = dir.getCurrentStepSize();
int microstep = dir.getIndex();
if (_debugging) {
Time currentTime = dir.getModelTime();
_debug(
"Fire at time "
+ currentTime
+ " and microstep "
+ microstep
+ " with step size "
+ stepSize);
}
// First handle the impulse input.
if (impulse.getWidth() > 0 && impulse.hasToken(0)) {
double impulseValue = ((DoubleToken) impulse.get(0)).doubleValue();
if (_debugging) {
_debug("-- impulse input received with value " + impulseValue);
}
if (impulseValue != 0.0) {
if (microstep == 0) {
throw new IllegalActionException(
this, "Signal at the impulse port is not purely discrete.");
}
double currentState = getState() + impulseValue;
setTentativeState(currentState);
if (_debugging) {
_debug("-- Due to impulse input, set state to " + currentState);
}
}
}
// Next handle the initialState port.
ParameterPort initialStatePort = initialState.getPort();
if (initialStatePort.getWidth() > 0 && initialStatePort.hasToken(0)) {
double initialValue = ((DoubleToken) initialStatePort.get(0)).doubleValue();
if (_debugging) {
_debug("-- initialState input received with value " + initialValue);
}
if (microstep == 0.0) {
throw new IllegalActionException(
this, "Signal at the initialState port is not purely discrete.");
}
setTentativeState(initialValue);
if (_debugging) {
_debug("-- Due to initialState input, set state to " + initialValue);
}
}
// Produce the current _tentativeState as output, if it
// has not already been produced.
if (!state.isKnown()) {
double tentativeOutput = getTentativeState();
// If the round has not updated since the last output, then
// just produce the same output as last time.
int currentRound = dir._getODESolver()._getRound();
if (_lastRound == currentRound) {
tentativeOutput = _lastOutput;
}
if (_debugging) {
_debug("** Sending output " + tentativeOutput);
}
_lastOutput = tentativeOutput;
state.broadcast(new DoubleToken(tentativeOutput));
}
// The _tentativeSate is committed only in postfire(),
// but multiple rounds will occur before postfire() is called.
// At each round, this fire() method may be called multiple
// times, and we want to make sure that the integration step
// only runs once in the step.
if (derivative.isKnown() && derivative.hasToken(0)) {
int currentRound = dir._getODESolver()._getRound();
if (_lastRound < currentRound) {
// This is the first fire() in a new round
// where the derivative input is known and present.
// Update the tentative state. Note that we will
// have already produced an output, and so we
// will not read the updated _tentativeState
// again in subsequent invocations of fire()
// in this round. So it is safe to update
// _tentativeState.
_lastRound = currentRound;
double currentDerivative = getDerivative();
if (Double.isNaN(currentDerivative) || Double.isInfinite(currentDerivative)) {
throw new IllegalActionException(
this, "The provided derivative input is invalid: " + currentDerivative);
}
if (stepSize > 0.0) {
// The following method changes the tentative state.
dir._getODESolver().integratorIntegrate(this);
}
}
}
}