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 MerweUnscentedTransformTest {
27  
28      /** test state dimension equal to 0 */
29      @Test(expected = MathIllegalArgumentException.class)
30      public void testWrongStateDimension() {
31          new MerweUnscentedTransform(0);
32      }
33  
34      /** test weight computation */
35      @Test
36      public void testWeights() {
37  
38          // Initialize
39          final int stateDim = 2;
40          final MerweUnscentedTransform merwe = new MerweUnscentedTransform(stateDim);
41          final RealVector wc = merwe.getWc();
42          final RealVector wm = merwe.getWm();
43  
44          // Verify
45          Assert.assertEquals(5,     wc.getDimension());
46          Assert.assertEquals(5,     wm.getDimension());
47          Assert.assertEquals(-0.25, wc.getEntry(0), Double.MIN_VALUE);
48          Assert.assertEquals(-3.0,  wm.getEntry(0), Double.MIN_VALUE);
49          Assert.assertEquals(1.0,   wc.getEntry(1), Double.MIN_VALUE);
50          Assert.assertEquals(1.0,   wm.getEntry(1), Double.MIN_VALUE);
51          Assert.assertEquals(1.0,   wc.getEntry(2), Double.MIN_VALUE);
52          Assert.assertEquals(1.0,   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 MerweUnscentedTransform merwe = new MerweUnscentedTransform(stateDim, 0.5, 2.0, 0.0);
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 = merwe.unscentedTransform(state, covariance);
68  
69          // Verify
70          Assert.assertEquals(5, sigma.length);
71          checkSigmaPoint(sigma[0], 1.0, 1.0);
72          checkSigmaPoint(sigma[1], 1.5, 1.0);
73          checkSigmaPoint(sigma[2], 1.0, 1.5);
74          checkSigmaPoint(sigma[3], 0.5, 1.0);
75          checkSigmaPoint(sigma[4], 1.0, 0.5);
76      }
77      
78      /** Test inverse unscented transform */
79      @Test
80      public void testInverseUnscentedTransform() {
81          
82          // Initialize
83          final int stateDim = 2;
84          final MerweUnscentedTransform merwe = new MerweUnscentedTransform(stateDim, 0.5, 2.0, 0.0);
85          final RealVector[] sigmaPoints = new RealVector[] {MatrixUtils.createRealVector(new double[] {1.0, 1.0}),
86                                                             MatrixUtils.createRealVector(new double[] {1.5, 1.0}),
87                                                             MatrixUtils.createRealVector(new double[] {1.0, 1.5}),
88                                                             MatrixUtils.createRealVector(new double[] {0.5, 1.0}),
89                                                             MatrixUtils.createRealVector(new double[] {1.0, 0.5})};
90          // Action
91          final Pair<RealVector, RealMatrix> inverse = merwe.inverseUnscentedTransform(sigmaPoints);
92          final RealVector state = inverse.getFirst();
93          final RealMatrix covariance = inverse.getSecond();
94          
95          // Verify
96          Assert.assertEquals(2, state.getDimension());
97          Assert.assertEquals(1.0, state.getEntry(0), 0.);
98          Assert.assertEquals(1.0, state.getEntry(1), 0.);
99          
100         Assert.assertEquals(2, covariance.getColumnDimension());
101         Assert.assertEquals(2, covariance.getRowDimension());
102         Assert.assertEquals(0.5, covariance.getEntry(0, 0), 0.);
103         Assert.assertEquals(0.0, covariance.getEntry(0, 1), 0.);
104         Assert.assertEquals(0.0, covariance.getEntry(1, 0), 0.);
105         Assert.assertEquals(0.5, covariance.getEntry(1, 1), 0.);
106     }
107 
108     private static void checkSigmaPoint(final RealVector sigma, final double ref1, final double ref2) {
109         Assert.assertEquals(ref1,  sigma.getEntry(0), Double.MIN_VALUE);
110         Assert.assertEquals(ref2,  sigma.getEntry(1), Double.MIN_VALUE);
111     }
112 
113 }