View Javadoc
1   /*
2    * Licensed to the Hipparchus project 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 Hipparchus project 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    *      https://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  package org.hipparchus.util;
18  
19  import org.hipparchus.exception.MathIllegalArgumentException;
20  import org.hipparchus.linear.MatrixUtils;
21  import org.hipparchus.linear.RealMatrix;
22  import org.hipparchus.linear.RealVector;
23  import org.junit.Assert;
24  import org.junit.Test;
25  
26  public class JulierUnscentedTransformTest {
27  
28      /** test state dimension equal to 0 */
29      @Test(expected = MathIllegalArgumentException.class)
30      public void testWrongStateDimension() {
31          new JulierUnscentedTransform(0);
32      }
33  
34      /** test weight computation */
35      @Test
36      public void testWeights() {
37  
38          // Initialize
39          final int stateDim = 2;
40          final JulierUnscentedTransform julier = new JulierUnscentedTransform(stateDim, 0.0);
41          final RealVector wc = julier.getWc();
42          final RealVector wm = julier.getWm();
43  
44          // Verify
45          Assert.assertEquals(5,    wc.getDimension());
46          Assert.assertEquals(5,    wm.getDimension());
47          Assert.assertEquals(0.0,  wc.getEntry(0), Double.MIN_VALUE);
48          Assert.assertEquals(0.0,  wm.getEntry(0), Double.MIN_VALUE);
49          Assert.assertEquals(0.25, wc.getEntry(1), Double.MIN_VALUE);
50          Assert.assertEquals(0.25, wm.getEntry(1), Double.MIN_VALUE);
51          Assert.assertEquals(0.25, wc.getEntry(2), Double.MIN_VALUE);
52          Assert.assertEquals(0.25, wm.getEntry(2), Double.MIN_VALUE);
53  
54      }
55  
56      /** Test unscented transform */
57      @Test
58      public void testUnscentedTransform() {
59  
60          // Initialize
61          final int stateDim = 2;
62          final JulierUnscentedTransform julier = new JulierUnscentedTransform(stateDim);
63          final RealVector state = MatrixUtils.createRealVector(new double[] {1.0, 1.0});
64          final RealMatrix covariance = MatrixUtils.createRealDiagonalMatrix(new double[] {0.5, 0.5});
65  
66          // Action
67          final RealVector[] sigma = julier.unscentedTransform(state, covariance);
68  
69          // Verify
70          Assert.assertEquals(5, sigma.length);
71          checkSigmaPoint(sigma[0], 1.0, 1.0);
72          checkSigmaPoint(sigma[1], 2.0, 1.0);
73          checkSigmaPoint(sigma[2], 1.0, 2.0);
74          checkSigmaPoint(sigma[3], 0.0, 1.0);
75          checkSigmaPoint(sigma[4], 1.0, 0.0);
76  
77      }
78  
79      /** Test inverse unscented transform */
80      @Test
81      public void testInverseUnscentedTransform() {
82          
83          // Initialize
84          final int stateDim = 2;
85          final JulierUnscentedTransform julier = new JulierUnscentedTransform(stateDim);
86          final RealVector[] sigmaPoints = new RealVector[] {MatrixUtils.createRealVector(new double[] {1.0, 1.0}),
87                                                             MatrixUtils.createRealVector(new double[] {2.0, 1.0}),
88                                                             MatrixUtils.createRealVector(new double[] {1.0, 2.0}),
89                                                             MatrixUtils.createRealVector(new double[] {0.0, 1.0}),
90                                                             MatrixUtils.createRealVector(new double[] {1.0, 0.0})};
91          // Action
92          final Pair<RealVector, RealMatrix> inverse = julier.inverseUnscentedTransform(sigmaPoints);
93          final RealVector state = inverse.getFirst();
94          final RealMatrix covariance = inverse.getSecond();
95          
96          // Verify
97          Assert.assertEquals(2, state.getDimension());
98          Assert.assertEquals(1.0, state.getEntry(0), 0.);
99          Assert.assertEquals(1.0, state.getEntry(1), 0.);
100         
101         Assert.assertEquals(2, covariance.getColumnDimension());
102         Assert.assertEquals(2, covariance.getRowDimension());
103         Assert.assertEquals(0.5, covariance.getEntry(0, 0), 0.);
104         Assert.assertEquals(0.0, covariance.getEntry(0, 1), 0.);
105         Assert.assertEquals(0.0, covariance.getEntry(1, 0), 0.);
106         Assert.assertEquals(0.5, covariance.getEntry(1, 1), 0.);
107     }
108 
109     private static void checkSigmaPoint(final RealVector sigma, final double ref1, final double ref2) {
110         Assert.assertEquals(ref1,  sigma.getEntry(0), Double.MIN_VALUE);
111         Assert.assertEquals(ref2,  sigma.getEntry(1), Double.MIN_VALUE);
112     }
113 
114 }