Mercurial > hg > de.mpg.mpiwg.itgroup.digilib.core

comparison libs/commons-math-2.1/docs/userguide/TrajectoryDeterminationProblem.java @ 10:5f2c5fb36e93

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
commons-math-2.1 added
author dwinter
date Tue, 04 Jan 2011 10:00:53 +0100
parents
children
comparison
equal deleted inserted replaced
9:e63a64652f4d 10:5f2c5fb36e93
1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * The ASF licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18 import org.apache.commons.math.optimization.general.EstimationException;
19 import org.apache.commons.math.optimization.general.EstimatedParameter;
20 import org.apache.commons.math.optimization.general.EstimationProblem;
21 import org.apache.commons.math.optimization.general.LevenbergMarquardtEstimator;
22 import org.apache.commons.math.optimization.general.SimpleEstimationProblem;
23 import org.apache.commons.math.optimization.general.WeightedMeasurement;
24
25 public class TrajectoryDeterminationProblem extends SimpleEstimationProblem {
26
27 public static void main(String[] args) {
28 try {
29 TrajectoryDeterminationProblem problem =
30 new TrajectoryDeterminationProblem(0.0, 100.0, 800.0, 1.0, 0.0);
31
32 double[][] distances = {
33 { 0.0, 806.5849 }, { 20.0, 796.8148 }, { 40.0, 791.0833 }, { 60.0, 789.6712 },
34 { 80.0, 793.1334 }, { 100.0, 797.7248 }, { 120.0, 803.2785 }, { 140.0, 813.4939 },
35 { 160.0, 826.9295 }, { 180.0, 844.0640 }, { 200.0, 863.3829 }, { 220.0, 883.3143 },
36 { 240.0, 908.6867 }, { 260.0, 934.8561 }, { 280.0, 964.0730 }, { 300.0, 992.1033 },
37 { 320.0, 1023.998 }, { 340.0, 1057.439 }, { 360.0, 1091.912 }, { 380.0, 1125.968 },
38 { 400.0, 1162.789 }, { 420.0, 1201.517 }, { 440.0, 1239.176 }, { 460.0, 1279.347 } };
39 for (int i = 0; i < distances.length; ++i) {
40 problem.addDistanceMeasurement(1.0, distances[i][0], distances[i][1]);
41 };
42
43 double[][] angles = {
44 { 10.0, 1.415423 }, { 30.0, 1.352643 }, { 50.0, 1.289290 }, { 70.0, 1.225249 },
45 { 90.0, 1.161203 }, {110.0, 1.098538 }, {130.0, 1.036263 }, {150.0, 0.976052 },
46 {170.0, 0.917921 }, {190.0, 0.861830 }, {210.0, 0.808237 }, {230.0, 0.757043 },
47 {250.0, 0.708650 }, {270.0, 0.662949 }, {290.0, 0.619903 }, {310.0, 0.579160 },
48 {330.0, 0.541033 }, {350.0, 0.505590 }, {370.0, 0.471746 }, {390.0, 0.440155 },
49 {410.0, 0.410522 }, {430.0, 0.382701 }, {450.0, 0.356957 }, {470.0, 0.332400 } };
50 for (int i = 0; i < angles.length; ++i) {
51 problem.addAngularMeasurement(3.0e7, angles[i][0], angles[i][1]);
52 };
53
54 LevenbergMarquardtEstimator estimator = new LevenbergMarquardtEstimator();
55 estimator.estimate(problem);
56 System.out.println("initial position: " + problem.getX0() + " " + problem.getY0());
57 System.out.println("velocity: " + problem.getVx0() + " " + problem.getVy0());
58
59 } catch (EstimationException ee) {
60 System.err.println(ee.getMessage());
61 }
62 }
63
64 public TrajectoryDeterminationProblem(double t0,
65 double x0Guess, double y0Guess,
66 double vx0Guess, double vy0Guess) {
67 this.t0 = t0;
68 x0 = new EstimatedParameter( "x0", x0Guess);
69 y0 = new EstimatedParameter( "y0", y0Guess);
70 vx0 = new EstimatedParameter("vx0", vx0Guess);
71 vy0 = new EstimatedParameter("vy0", vy0Guess);
72
73 // inform the base class about the parameters
74 addParameter(x0);
75 addParameter(y0);
76 addParameter(vx0);
77 addParameter(vy0);
78
79 }
80
81 public double getX0() {
82 return x0.getEstimate();
83 }
84
85 public double getY0() {
86 return y0.getEstimate();
87 }
88
89 public double getVx0() {
90 return vx0.getEstimate();
91 }
92
93 public double getVy0() {
94 return vy0.getEstimate();
95 }
96
97 public void addAngularMeasurement(double wi, double ti, double ai) {
98 // let the base class handle the measurement
99 addMeasurement(new AngularMeasurement(wi, ti, ai));
100 }
101
102 public void addDistanceMeasurement(double wi, double ti, double di) {
103 // let the base class handle the measurement
104 addMeasurement(new DistanceMeasurement(wi, ti, di));
105 }
106
107 public double x(double t) {
108 return x0.getEstimate() + (t - t0) * vx0.getEstimate();
109 }
110
111 public double y(double t) {
112 return y0.getEstimate() + (t - t0) * vy0.getEstimate();
113 }
114
115 private class AngularMeasurement extends WeightedMeasurement {
116
117 public AngularMeasurement(double weight, double t, double angle) {
118 super(weight, angle);
119 this.t = t;
120 }
121
122 public double getTheoreticalValue() {
123 return Math.atan2(y(t), x(t));
124 }
125
126 public double getPartial(EstimatedParameter parameter) {
127 double xt = x(t);
128 double yt = y(t);
129 double r = Math.sqrt(xt * xt + yt * yt);
130 double u = yt / (r + xt);
131 double c = 2 * u / (1 + u * u);
132 if (parameter == x0) {
133 return -c;
134 } else if (parameter == vx0) {
135 return -c * t;
136 } else if (parameter == y0) {
137 return c * xt / yt;
138 } else {
139 return c * t * xt / yt;
140 }
141 }
142
143 private final double t;
144 private static final long serialVersionUID = -5990040582592763282L;
145
146 }
147
148 private class DistanceMeasurement extends WeightedMeasurement {
149
150 public DistanceMeasurement(double weight, double t, double angle) {
151 super(weight, angle);
152 this.t = t;
153 }
154
155 public double getTheoreticalValue() {
156 double xt = x(t);
157 double yt = y(t);
158 return Math.sqrt(xt * xt + yt * yt);
159 }
160
161 public double getPartial(EstimatedParameter parameter) {
162 double xt = x(t);
163 double yt = y(t);
164 double r = Math.sqrt(xt * xt + yt * yt);
165 if (parameter == x0) {
166 return xt / r;
167 } else if (parameter == vx0) {
168 return xt * t / r;
169 } else if (parameter == y0) {
170 return yt / r;
171 } else {
172 return yt * t / r;
173 }
174 }
175
176 private final double t;
177 private static final long serialVersionUID = 3257286197740459503L;
178
179 }
180
181 private double t0;
182 private EstimatedParameter x0;
183 private EstimatedParameter y0;
184 private EstimatedParameter vx0;
185 private EstimatedParameter vy0;
186
187 }