|
10
|
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 }
|
