SensorLog

SensorLog.java

import java.io.BufferedReader;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

/**
 * Represents a sensor log file.
 *
 * Command line usage:
 *
 * $ java SensorLog [-t time_step] [sensor_index...] filename
 *
 * -t time_step provides the time step as a floating point number (default:
 * 0.005) sensor_index a list of sensor index filename filename or - to read
 * from the standard input
 *
 * Example usage in command line to compute the jerk of the first, second and
 * third sensors (assuming those sensors are accelerometers):
 *
 * $ java SensorLog sensorLog.txt
 *
 * The sensors to be read can be changed by providing their indices before the
 * name of the sensor log file. Example for reading the fourth and fifth
 * sensors:
 *
 * $ java SensorLog 3 4 sensorLog.txt
 *
 * Set the simulation time step to 0.01:
 *
 * $ java SensorLog -t 0.01 3 4 sensorLog.txt
 *
 * @author Arthur Gay <arthur.gay@epfl.ch>
 */
class SensorLog {

    /**
     * Stores the values of all the sensors.
     */
    final double values[][];
    /**
     * The time step of the simulation.
     */
    final double timeStep;

    /**
     * Parse the given input stream with a default timeStep equals to 0.005s.
     *
     * @param is Input Stream for reading the sensor log file
     * @throws SensorLog.MalformedFileException when the file is not correctly
     * formatted
     * @throws IOException
     */
    SensorLog(InputStream is) throws MalformedFileException, IOException {
        this(is, 0.005);
    }

    /**
     * Parse the given input stream.
     *
     * @param is Input Stream for reading the sensor log file
     * @param timeStep of the simulation
     * @throws SensorLog.MalformedFileException when the file is not correctly
     * formatted
     * @throws IOException
     */
    SensorLog(InputStream is, double timeStep) throws MalformedFileException, IOException {
        /*
         * Parsing is done in two steps:
         *  1) Read the file line by line. Each line is splitted by space and
         *     the resulting list of tokens is stored into a list of lists of
         *     tokens.
         *  2) Read the list of lists of tokens and parse each token into a
         *     double and store it into an two dimensional array.
         */
        BufferedReader reader;
        String line;
        String splitLine[];
        List<String> spaceSeparatedValuesLine;
        List<List<String>> spaceSeparatedValues = new ArrayList<>();
        int lineSensorCount;
        // Set to -1 to signal that the variable has not been initialised yet
        int sensorCount = -1;
        int lineCount = 0;

        this.timeStep = timeStep;

        reader = new BufferedReader(new InputStreamReader(is));
        line = reader.readLine();

        while (line != null) {
            lineCount += 1;
            splitLine = line.trim().split("\\s+");
            spaceSeparatedValuesLine = Arrays.asList(splitLine);
            lineSensorCount = splitLine.length;

            // each line must have the same number of sensors
            if (sensorCount == -1) {
                sensorCount = lineSensorCount;
            } else if (sensorCount != lineSensorCount) {
                throw new MalformedFileException(String.format("Error line %d: expected %d sensors, got %d.",
                        lineCount, sensorCount, lineSensorCount));
            }

            spaceSeparatedValues.add(spaceSeparatedValuesLine);
            line = reader.readLine();
        }

        values = new double[sensorCount][lineCount];
        for (int i = 0; i < sensorCount; i++) {
            for (int j = 0; j < lineCount; j++) {
                values[i][j] = Double.parseDouble(spaceSeparatedValues.get(j).get(i));
            }
        }
    }

    /**
     * Compute the mean of the absolute difference divided by the time step.
     *
     * Let n the number of samples and f(k) the kth value of the sensor, with 0
     * &lt; k &lt; n. Let h the time step. The difference d(k) is computed using
     * the following formula:
     *
     * d(k) = |f(k+1) - f(k)| / h
     *
     * The mean is then computed using the following formula:
     *
     * m = sum d(i) / (n - 1), i = 0 to n-1
     *
     * @param sensorIndex
     * @return
     */
    double computeMeanAbsoluteDifference(int sensorIndex) {
        double sum = 0.0;
        double sensorValues[] = values[sensorIndex];

        for (int i = 0; i < sensorValues.length - 1; i++) {
            sum += Math.abs(sensorValues[i + 1] - sensorValues[i]) / this.timeStep;
        }
        return sum / (sensorValues.length - 1);
    }

    /**
     * Compute the euclidian norm of a vector of mean absolute differences.
     *
     * Compute the mean of the absolute forward difference divided by the
     * timeStep of multiple sensors and compute their euclidian norm using the
     * following formula:
     *
     * n = sqrt(x^2 + y^2 + ...)
     *
     * @param sensorIndices
     * @return
     */
    double computeMeanAbsoluteDifferenceVector(int... sensorIndices) {
        double sum = 0.0;

        for (int i = 0; i < sensorIndices.length; i++) {
            sum += computeMeanAbsoluteDifference(sensorIndices[i])
                * computeMeanAbsoluteDifference(sensorIndices[i]);
        }

        return Math.sqrt(sum);
    }

    public static void main(String args[]) {
        String filename;
        SensorLog sensorLog;
        // Use standard input by default
        InputStream inputStream = System.in;
        // Use the 3 first sensors by default
        int sensorIndices[] = {0, 1, 2};
        double timeStep = 0.005;
        // A "shift" for parsing command line arguments
        int argShift = 0;

        if (args.length < 1) {
            System.err.println("Error: no filename specified. Aborting.");
            System.exit(10);
        }

        filename = args[args.length - 1];

        // The time step option must be the first option. I keep it simple here.
        if (args[0].equals("-t")) {
            if (args.length >= 2) {
                timeStep = Double.parseDouble(args[1]);
                argShift += 2;
            } else {
                System.err.println("Error: no timestep specified. Aborting.");
                System.exit(15);
            }
        }

        // Read sensor indices
        if (args.length - argShift > 1) {
            sensorIndices = new int[args.length - 1 - argShift];
            for (int i = 0; i < args.length - 1 - argShift; i++) {
                sensorIndices[i] = Integer.parseInt(args[i + argShift]);
            }
        }

        // If filename is -, use default input
        if (!filename.equals("-")) {
            try {
                inputStream = new FileInputStream(filename);
            } catch (FileNotFoundException ex) {
                System.err.format("Error: file '%s' not found. Aborting.\n", filename);
                System.exit(20);
            }
        }

        try {
            sensorLog = new SensorLog(inputStream, timeStep);

            // Print output
            System.out.println(sensorLog.computeMeanAbsoluteDifferenceVector(sensorIndices));
        } catch (IOException ex) {
            ex.printStackTrace(); // keep it for easy debugging
            System.exit(30);
        } catch (MalformedFileException ex) {
            System.err.format("Error: File is malformed: %s\n", ex.getMessage());
            System.exit(40);
        }
    }

    class MalformedFileException extends Exception {

        public MalformedFileException(String message) {
            super(message);
        }
    }
}

sensorLog.m

% Computation using matlab or octave
x = [
    1.23418e-25  -7.5133e-23         9.81
    0.164183 -0.000411184      33.6536
    0.000620223  7.63902e-07      9.81015
    0.000616464 -1.45264e-06         9.81
    0.000616464 -1.45264e-06         9.81
    0.000616464 -1.45264e-06         9.81
    0.000616464 -1.45264e-06         9.81
    0.000616464 -1.45264e-06         9.81
    0.000616464 -1.45264e-06         9.81
    0.000616464 -1.45264e-06         9.81
];

norm([mean(abs(diff(x(:, 1)))), mean(abs(diff(x(:, 2)))), mean(abs(diff(x(:, 3))))]/0.005)

sensorLog.txt

 1.23418e-25  -7.5133e-23         9.81
    0.164183 -0.000411184      33.6536
 0.000620223  7.63902e-07      9.81015
 0.000616464 -1.45264e-06         9.81
 0.000616464 -1.45264e-06         9.81
 0.000616464 -1.45264e-06         9.81
 0.000616464 -1.45264e-06         9.81
 0.000616464 -1.45264e-06         9.81
 0.000616464 -1.45264e-06         9.81
 0.000616464 -1.45264e-06         9.81