--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/libs/commons-math-2.1/docs/userguide/TrajectoryDeterminationProblem.java	Tue Jan 04 10:00:53 2011 +0100
@@ -0,0 +1,187 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License.  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+import org.apache.commons.math.optimization.general.EstimationException;
+import org.apache.commons.math.optimization.general.EstimatedParameter;
+import org.apache.commons.math.optimization.general.EstimationProblem;
+import org.apache.commons.math.optimization.general.LevenbergMarquardtEstimator;
+import org.apache.commons.math.optimization.general.SimpleEstimationProblem;
+import org.apache.commons.math.optimization.general.WeightedMeasurement;
+
+public class TrajectoryDeterminationProblem extends SimpleEstimationProblem {
+
+    public static void main(String[] args) {
+        try {
+            TrajectoryDeterminationProblem problem =
+              new TrajectoryDeterminationProblem(0.0, 100.0, 800.0, 1.0, 0.0);
+
+            double[][] distances = {
+                    {   0.0, 806.5849 }, {  20.0, 796.8148 }, {  40.0, 791.0833 }, {  60.0, 789.6712 },
+                    {  80.0, 793.1334 }, { 100.0, 797.7248 }, { 120.0, 803.2785 }, { 140.0, 813.4939 },
+                    { 160.0, 826.9295 }, { 180.0, 844.0640 }, { 200.0, 863.3829 }, { 220.0, 883.3143 },
+                    { 240.0, 908.6867 }, { 260.0, 934.8561 }, { 280.0, 964.0730 }, { 300.0, 992.1033 },
+                    { 320.0, 1023.998 }, { 340.0, 1057.439 }, { 360.0, 1091.912 }, { 380.0, 1125.968 },
+                    { 400.0, 1162.789 }, { 420.0, 1201.517 }, { 440.0, 1239.176 }, { 460.0, 1279.347 } };
+            for (int i = 0; i < distances.length; ++i) {
+                problem.addDistanceMeasurement(1.0,  distances[i][0], distances[i][1]);
+            };
+
+            double[][] angles = {
+                    { 10.0, 1.415423 }, { 30.0, 1.352643 }, { 50.0, 1.289290 }, { 70.0, 1.225249 },
+                    { 90.0, 1.161203 }, {110.0, 1.098538 }, {130.0, 1.036263 }, {150.0, 0.976052 },
+                    {170.0, 0.917921 }, {190.0, 0.861830 }, {210.0, 0.808237 }, {230.0, 0.757043 },
+                    {250.0, 0.708650 }, {270.0, 0.662949 }, {290.0, 0.619903 }, {310.0, 0.579160 },
+                    {330.0, 0.541033 }, {350.0, 0.505590 }, {370.0, 0.471746 }, {390.0, 0.440155 },
+                    {410.0, 0.410522 }, {430.0, 0.382701 }, {450.0, 0.356957 }, {470.0, 0.332400 } };
+            for (int i = 0; i < angles.length; ++i) {
+                problem.addAngularMeasurement(3.0e7, angles[i][0], angles[i][1]);
+            };
+
+            LevenbergMarquardtEstimator estimator = new LevenbergMarquardtEstimator();
+            estimator.estimate(problem);
+            System.out.println("initial position: " + problem.getX0() + " " + problem.getY0());
+            System.out.println("velocity: " + problem.getVx0() + " " + problem.getVy0());
+
+        } catch (EstimationException ee) {
+          System.err.println(ee.getMessage());
+        }
+    }
+
+    public TrajectoryDeterminationProblem(double t0,
+                                          double  x0Guess, double  y0Guess,
+                                          double vx0Guess, double vy0Guess) {
+        this.t0 = t0;
+         x0 = new EstimatedParameter( "x0",  x0Guess);
+         y0 = new EstimatedParameter( "y0",  y0Guess);
+        vx0 = new EstimatedParameter("vx0", vx0Guess);
+        vy0 = new EstimatedParameter("vy0", vy0Guess);
+
+        // inform the base class about the parameters
+        addParameter(x0);
+        addParameter(y0);
+        addParameter(vx0);
+        addParameter(vy0);
+
+    }
+
+    public double getX0() {
+        return x0.getEstimate();
+    }
+
+    public double getY0() {
+        return y0.getEstimate();
+    }
+
+    public double getVx0() {
+        return vx0.getEstimate();
+    }
+
+    public double getVy0() {
+        return vy0.getEstimate();
+    }
+
+    public void addAngularMeasurement(double wi, double ti, double ai) {
+        // let the base class handle the measurement
+        addMeasurement(new AngularMeasurement(wi, ti, ai));
+    }
+
+    public void addDistanceMeasurement(double wi, double ti, double di) {
+        // let the base class handle the measurement
+        addMeasurement(new DistanceMeasurement(wi, ti, di));
+    }
+
+    public double x(double t) {
+        return x0.getEstimate() + (t - t0) * vx0.getEstimate();
+    }
+
+    public double y(double t) {
+        return y0.getEstimate() + (t - t0) * vy0.getEstimate();
+    }
+
+    private class AngularMeasurement extends WeightedMeasurement {
+
+        public AngularMeasurement(double weight, double t, double angle) {
+            super(weight, angle);
+            this.t = t;
+        }
+
+        public double getTheoreticalValue() {
+            return Math.atan2(y(t), x(t));
+        }
+
+        public double getPartial(EstimatedParameter parameter) {
+            double xt = x(t);
+            double yt = y(t);
+            double r  = Math.sqrt(xt * xt + yt * yt);
+            double u  = yt / (r + xt);
+            double c  = 2 * u / (1 + u * u);
+            if (parameter == x0) {
+                return -c;
+            } else if (parameter == vx0) {
+                return -c * t;
+            } else if (parameter == y0) {
+                return c * xt / yt;
+            } else {
+                return c * t * xt / yt;
+            }
+        }
+
+        private final double t;
+        private static final long serialVersionUID = -5990040582592763282L;
+
+    }
+
+    private class DistanceMeasurement extends WeightedMeasurement {
+
+        public DistanceMeasurement(double weight, double t, double angle) {
+            super(weight, angle);
+            this.t = t;
+        }
+
+        public double getTheoreticalValue() {
+            double xt = x(t);
+            double yt = y(t);
+            return Math.sqrt(xt * xt + yt * yt);
+        }
+
+        public double getPartial(EstimatedParameter parameter) {
+            double xt = x(t);
+            double yt = y(t);
+            double r  = Math.sqrt(xt * xt + yt * yt);
+            if (parameter == x0) {
+                return xt / r;
+            } else if (parameter == vx0) {
+                return xt * t / r;
+            } else if (parameter == y0) {
+                return yt / r;
+            } else {
+                return yt * t / r;
+            }
+        }
+
+        private final double t;
+        private static final long serialVersionUID = 3257286197740459503L;
+
+    }
+
+    private double t0;
+    private EstimatedParameter x0;
+    private EstimatedParameter y0;
+    private EstimatedParameter vx0;
+    private EstimatedParameter vy0;
+
+}