edu.nps.moves.deadreckoning
Class DIS_DeadReckoning

java.lang.Object
  edu.nps.moves.deadreckoning.DIS_DeadReckoning

All Implemented Interfaces:

java.lang.Runnable

Direct Known Subclasses:

DIS_DR_FPB_06, DIS_DR_FPW_02, DIS_DR_FVB_09, DIS_DR_FVW_05, DIS_DR_RPB_07, DIS_DR_RPW_03, DIS_DR_RPW_03b, DIS_DR_RVB_08, DIS_DR_RVW_04, DIS_DR_RVW_04b, DIS_DR_Static_01

public abstract class DIS_DeadReckoning
extends java.lang.Object
implements java.lang.Runnable

The root super class for all DIS Dead-Reckoning algorithms. Based on the algrorithms from the IEEE 1278_1-1995_DIS standards found in Annex B.

Final project presentation

Creates an abstract instance of a Dead Reckoning (DR) algorithm, defined by the concrete Dead Reckoning algorithm on the right hand side.

At each PDU update received, call the set function to update the DR algorithm with the most accurance and update information. Expected to receive a new update approx every 5 seconds or so. Each PDU is essentally a restart or reset of the DR state.

The DR wroks off the last good state (origin) and extrapulates out from that point based on the velocity and acceleration parameters from the set function.

The DR algorithm updates 30 times a second. The instantiating entity can get updated DR states at its leasure upto 30 times a second by calling the get function, which returns an array of 6 doubles 3 x location and 3 x orientation. With these 6 parameters the entity can redraw itslef in an updatedloation and orientsation based on its projected path.

Keynotes form the IEEE DIS standard about DR

DRM notation shall consist of three elements.

The First element shall indicate whether the model specifies rotation as either fixed (F) or rotating (R).

The second element shall specify dead reckoning rates to be held constant as either rate of position (P) or rate of velocity (V).

The third element shall specify the coordinate system to be used with the dead reckoning algorithm as either world coordinates (W) or body axis coordinates (B).

5.2.1 Angle representation
Angles shall be specfified as 32-bit floating point numbers expressed in radians.(page 55)

5.2.2 Angular Velocity Vector record
The angular velocity of simulated entities shall be represented by the Angular Velocity Vector record. This record shall specify the rate at which an entity's orientation is changing. The angular velocity shall be measured in radians per second measured about each of the entity's own coordinate axes. The record shall consist of three fields. The first field shall represent velocity about the x-axis, the second about the y-axis, and the third about the z-axis [see 5.2.33 item a)]. The positive direction of the angular velocity is defined by the right-hand rule. The format of the Angular Velocity Vector record shall be shown as in table 5. (Page 55)

5.2.17 Euler Angles record
Orientation of a simulated entity shall be specified by the Euler Angles record. This record shall specify three angles as described in figure 3 and 3.1.13. These angles shall be specified with respect to the entity's coordinate system. The three angles shall each be specified by a 32-bit floating point number representing radians. The format of the Euler Angles record shall be as shown in table 19. (page 65)

5.2.33 Vector record
Vector values for entity coordinates, linear acceleration, and linear velocity, shall be represented using a Vector record. This record shall consist of three fields, each a 32-bit floating point number. The unit of measure represented by these fields shall depend on the information represented. The values utilizing the Vector record are as follows:

a) Entity Coordinate Vector. Location with respect to a particular entity shall be specified with respect to three orthogonal axes whose origin shall be the geometric center of the bounding volume of the entity excluding its articulated and attached parts (see figure 2). The x-axis extends in the positive direction out the front of the entity. The y-axis extends in the positive direction out the right side of the entity as viewed from above, facing in the direction of the positive x-axis. The z-axis extends in the positive direction out the bottom of the entity. Each vector component shall represent meters from the origin (see figure 2).

b) Linear Acceleration Vector. Linear acceleration shall be represented as a vector with components in either world coordinate system or entity's coordinate system depending on the value in the Dead Reckoning Algorithm field. Each vector component shall represent acceleration in meters per second squared.

c) Linear Velocity Vector. Linear velocity shall be represented as a vector with three components in either world coordinate system or entity's coordinate system depending on the value in the Dead Reckoning Algorithm field. Each vector component shall represent velocity in meters per second. The format of the Vector record shall be as shown in table 30. (page 73)

5.2.34 World Coordinates record
Location of the origin of the entity's coordinate system shall be specified by a set of three coordinates: X, Y, and Z. The shape of the earth shall be specified using DMA TR 8350.2, 1987. The origin of the world coordinate system shall be the centroid of the earth, with the X-axis passing through the prime meridian at the equator, the Y-axis passing through 90° east longitude at the equator, and the Z-axis passing through the north pole (see figure 1). These coordinates shall represent meters from the centroid of the earth. A 64-bit double precision floating point number shall represent the location for each coordinate.

The format of the World Coordinates record shall be as shown in table 31. (page 73)

The Dead Reckoning parameters captured from each PDU

The IEEE specified algorithms to compute the DR for Primary Methods Group (1-5)

REVISED POSITION

The Position portion of the algorithms

ORIENTATION SOLVER

Ultimately, the PSI (rotation about the y-axis), THETA (rotation about the z-axis), PHI (rotation about the x-axis) need to be in the range of 0 - 2PI since the fields are in radians.

The Orientation portion of the algorithms

DR MATRIX SOLVER

The generic DR matrix

Graphics rotate (x,y,z) matrices
= [DR]
ultimately what this is DR equation is doing but with a change of basis from world to entity coordinates.

The angular velocity Magnitude

The SKEW matrix

The angular velocity Matrix

NOTE - It was mentioned above that the angular velocities are contained in the Entity State PDU as body axis velocities. However, if the angular velocities are in terms of the Euler angles, then a transformation to body axis angular velocities is needed. Thus the following transformation formulas are given:

Body to Wrold Transformation

World to Body Transformation

R MATRIX SOLVER

Initial Orientation Matrix

REVISED ORIENTATION

Get the Revised Orientation

The IEEE specified algorithms to compute the DR for Secondary Methods Group (6-9)

Note: the Rotaion formula where applicable is the same as that used in the Primary Methods Group (1-5), as well is the equation for getting the revised orientations.

General position formula

R1 vector (though I am not sure what its really calculating)

R2 vector (though I am not sure what its really calculating)

An Example:

import DIS.DeadReconing.*;

public class runTest 
{
    public static void main(String s[])
    {
        // create a DeadReconing Entity
        DIS_DeadReckoning dr = new DIS_DR_FPW_02();

        // make the arrays of location and other parameters
        //                loc      orien    lin V    Accel    Ang V
        double[] locOr = {2,3,4,   5,6,1,   1,2,1,   0,0,0,   0,0,0};

        // set the parameters
        dr.setNewAll(locOr);

        // Print out the current state
        System.out.println(dr.toString());
        System.out.println();

        try
        {
            // wait 1 second
            Thread.sleep(1000);

            // request an update from the DR algorith
            // should be original + 1 full value of other parameters
            // new position should be (3, 5, 5)
            double[] update = dr.getUpdatedPositionOrientation();

            // print the update to the screen
            System.out.println(dr.toString());        
        }
        catch(Exception e)
        {
            System.out.println("Unknow Error...?\n    " + e);
        }

        // terminate with exit to get out of the inf while loop
        System.exit(0);
    }
}

Resulting Output:
Current State of this Entity:
    Entity Location = (2.0, 3.0, 4.0)
    Entity Orientation = (5.0, 6.0, 1.0)
    Entity Linear Velocity = (1.0, 2.0, 1.0)
    Entity Linear Acceleration = (0.0, 0.0, 0.0)
    Entity Angular Velocity = (0.0, 0.0, 0.0)
    Delta between updates = 0.033333335

Current State of this Entity:
    Entity Location = (3.000000052154064, 5.000000104308128, 5.000000052154064)
    Entity Orientation = (5.0, 6.0, 1.0)
    Entity Linear Velocity = (1.0, 2.0, 1.0)
    Entity Linear Acceleration = (0.0, 0.0, 0.0)
    Entity Angular Velocity = (0.0, 0.0, 0.0)
    Delta between updates = 0.033333335

Author:

Sheldon L. Snyder

Field Summary

protected java.lang.Thread	aThread Thread for the DR algorithm update timing (1/30 second)
protected float	changeDelta How far to change the location/orientation per update
protected int	deltaCt How many updates have occured ...
protected float	entityAngularVelocity_X The X angular velocity 32bit float
protected float	entityAngularVelocity_Y The Y angular velocity 32bit float
protected float	entityAngularVelocity_Z The Z angular velocity 32bit float
protected float	entityLinearAcceleration_X The linear X acceleration 32bit float
protected float	entityLinearAcceleration_Y The linear Y acceleration 32bit float
protected float	entityLinearAcceleration_Z The linear Z acceleration 32bit float
protected float	entityLinearVelocity_X The X linear velocity 32bit float
protected float	entityLinearVelocity_Y The Y linear velocity 32bit float
protected float	entityLinearVelocity_Z The Z linear velocity 32bit float
protected double	entityLocation_X The entity's X coordinate location with double percision 64bit
protected double	entityLocation_Y The entity's Y coordinate location with double percision 64bit
protected double	entityLocation_Z The entity's Z coordinate location with double percision 64bit
protected float	entityOrientation_phi The Z orientation of the entity with 32bit float
protected float	entityOrientation_psi The X orientation of the entity with 32bit float
protected float	entityOrientation_theta The Y orientation of the entity with 32bit float
protected float	fps how may times per second to update this entity's positon
protected long	stall How long to wait between updates

Constructor Summary

DIS_DeadReckoning()
Constructor for all DR algorithms...

Method Summary

double[]	getUpdatedPositionOrientation() Gets the revised position and orientation of this entity
void	setFPS(int frames) Sets the refresh rate for the scene.
void	setNewAll(double[] allDis) Set the parameters for this entity's DR function based on the most recent PDU.
java.lang.String	toString() Pretty print the current state of this Dead Reckoning object

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, wait, wait, wait

Methods inherited from interface java.lang.Runnable

run

Field Detail

entityLocation_X

protected double entityLocation_X

The entity's X coordinate location with double percision 64bit

entityLocation_Y

protected double entityLocation_Y

The entity's Y coordinate location with double percision 64bit

entityLocation_Z

protected double entityLocation_Z

The entity's Z coordinate location with double percision 64bit

entityOrientation_psi

protected float entityOrientation_psi

The X orientation of the entity with 32bit float

entityOrientation_theta

protected float entityOrientation_theta

The Y orientation of the entity with 32bit float

entityOrientation_phi

protected float entityOrientation_phi

The Z orientation of the entity with 32bit float

entityLinearVelocity_X

protected float entityLinearVelocity_X

The X linear velocity 32bit float

entityLinearVelocity_Y

protected float entityLinearVelocity_Y

The Y linear velocity 32bit float

entityLinearVelocity_Z

protected float entityLinearVelocity_Z

The Z linear velocity 32bit float

entityLinearAcceleration_X

protected float entityLinearAcceleration_X

The linear X acceleration 32bit float

entityLinearAcceleration_Y

protected float entityLinearAcceleration_Y

The linear Y acceleration 32bit float

entityLinearAcceleration_Z

protected float entityLinearAcceleration_Z

The linear Z acceleration 32bit float

entityAngularVelocity_X

protected float entityAngularVelocity_X

The X angular velocity 32bit float

entityAngularVelocity_Y

protected float entityAngularVelocity_Y

The Y angular velocity 32bit float

entityAngularVelocity_Z

protected float entityAngularVelocity_Z

The Z angular velocity 32bit float

fps

protected float fps

how may times per second to update this entity's positon

changeDelta

protected float changeDelta

How far to change the location/orientation per update

deltaCt

protected int deltaCt

How many updates have occured ... only used for testing

Reset to 0 with each call to setAll()

stall

protected long stall

How long to wait between updates

the delta between calls...how fast an entity will be updated

1. Assumed a desired rate of 30 fps
2. Given from the standard that all parameters are in meters/s
3. To move 1 meter/second with 30 incriments = 1/30 Delta between updates
4. delay in milli seconds is 1/30 * 1000 || 1000 / 30

Note from Java Doc for JDK:
Causes the currently executing thread to sleep (temporarily cease execution) for the specified number of milliseconds, subject to the precision and accuracy of system timers and schedulers. The thread does not lose ownership of any monitors.

aThread

protected java.lang.Thread aThread

Thread for the DR algorithm update timing (1/30 second)

Constructor Detail

DIS_DeadReckoning

public DIS_DeadReckoning()

Constructor for all DR algorithms...

Each subclass DR algorithm has a no arguments constructor, but all it does is call the super, i.e. this constructor, which establishes the Thread

Method Detail

getUpdatedPositionOrientation

public double[] getUpdatedPositionOrientation()

Gets the revised position and orientation of this entity

Applies the required DR formula to the initial position and orientation of this entity and returns the updated locaiton and position.

This function does not actually perform the computations, it only returns the current state of the entity. The entity state is updated byt the specified DR alorithm within the DR class behind the scenes. Updates are crated every 1/30 seconds.

1. Assume a desire of 30 fps
2. All parameters are in meters/s
3. to move 1 meter/second with 30 incriments = 1/30 Delta between updates

   Only returns an array of location and orientation because that is all that is needed to update the location of the entity. All other DR inputs are parameters for solving the locaiton and orientation and so are not returned, only set.

   Order of the retruned array elements

   1. entityLocation_X
   2. entityLocation_Y
   3. entityLocation_Z
   4. entityOrientation_psi
   5. entityOrientation_theta
   6. entityOrientation_phi

   Returns:

   - 6 doubles of location and orientation

setFPS

public void setFPS(int frames)

Sets the refresh rate for the scene.

Default is 30 but can be changed throught this function call

Parameters:

frames - - the number of updats per second to make

setNewAll

public void setNewAll(double[] allDis)
               throws java.lang.Exception

Set the parameters for this entity's DR function based on the most recent PDU.

This ic called by the entity anytime the entity receives an updated ESPDU for this entity.

This can be the first and initialization call or update.

The folowing (triples) are set with each call:

1. Entity Locaiton 64bit
2. Entity Orientation 32bit
3. Entity Linear Velocity 32bit
4. Entity Linear Acceleration 32bit
5. Entity Angular Velocity 32bit

entityLocation_X = allDis[0];
entityLocation_Y = allDis[1];
entityLocation_Z = allDis[2];

entityOrientation_psi = (float)allDis[3];
entityOrientation_theta = (float)allDis[4];
entityOrientation_phi = (float)allDis[5];

entityLinearVelocity_X = (float)allDis[6];
entityLinearVelocity_Y = (float)allDis[7];
entityLinearVelocity_Z = (float)allDis[8];

entityLinearAcceleration_X = (float)allDis[9];
entityLinearAcceleration_Y = (float)allDis[10];
entityLinearAcceleration_Z = (float)allDis[11];

entityAngularVelocity_X = (float)allDis[12];
entityAngularVelocity_Y = (float)allDis[13];
entityAngularVelocity_Z = (float)allDis[14];

DR fields from a PDU update or initial

Parameters:

allDis - - 15 double percisions representing the above in order of the above

Throws:

java.lang.Exception

toString

public java.lang.String toString()

Pretty print the current state of this Dead Reckoning object

Updates are not included in this call, this is only the state.

Overrides:

toString in class java.lang.Object

Returns:

- String of pretty print of this DR entity