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 julier = 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 = 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], 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  
79      private static void checkSigmaPoint(final RealVector sigma, final double ref1, final double ref2) {
80          Assert.assertEquals(ref1,  sigma.getEntry(0), Double.MIN_VALUE);
81          Assert.assertEquals(ref2,  sigma.getEntry(1), Double.MIN_VALUE);
82      }
83  
84  }