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.LocalizedCoreFormats;
20 import org.hipparchus.exception.MathIllegalArgumentException;
21 import org.hipparchus.linear.ArrayRealVector;
22 import org.hipparchus.linear.RealMatrix;
23 import org.hipparchus.linear.RealVector;
24 import org.hipparchus.linear.SemiDefinitePositiveCholeskyDecomposition;
25
26 /**
27 * Base class for unscented transform providers.
28 * @since 2.2
29 */
30 public abstract class AbstractUnscentedTransform implements UnscentedTransformProvider {
31
32 /**
33 * Constructor.
34 * @param stateDim the dimension of the state
35 */
36 public AbstractUnscentedTransform(final int stateDim) {
37 // Check state dimension
38 if (stateDim == 0) {
39 // State dimension must be different from 0
40 throw new MathIllegalArgumentException(LocalizedCoreFormats.ZERO_STATE_SIZE);
41 }
42 }
43
44 /**
45 * {@inheritDoc}
46 * <p>
47 * Let n be the state dimension and Si be the ith row of the covariance matrix square root.
48 * The returned array is organized as follow. Element 0 contains the process state, also
49 * called the mean state. Elements from 1 to n contain the process state + Si. Finally,
50 * elements from n + 1 to 2n contain the process state - Si
51 */
52 @Override
53 public RealVector[] unscentedTransform(final RealVector state, final RealMatrix covariance) {
54
55 // State dimensions
56 final int n = state.getDimension();
57
58 // Initialize array containing sigma points
59 final RealVector[] sigmaPoints = new ArrayRealVector[(2 * n) + 1];
60 sigmaPoints[0] = state;
61
62 // Apply multiplication factor to the covariance matrix
63 final double factor = getMultiplicationFactor();
64 final RealMatrix temp = covariance.scalarMultiply(factor);
65
66 // Compute lower triangular matrix of Cholesky decomposition
67 // Note: When the estimation error covariance is propagated, it sometimes
68 // cannot maintain the positive semidefiniteness.
69 // To enhance the numerical stability of the unscented transform,
70 // the semidefinite positive Cholesky decomposition is used.
71 final RealMatrix L = new SemiDefinitePositiveCholeskyDecomposition(temp).getL();
72
73 // Compute sigma points
74 for (int i = 1; i <= n; i++) {
75 sigmaPoints[i] = sigmaPoints[0].add(L.getColumnVector(i - 1));
76 sigmaPoints[i + n] = sigmaPoints[0].subtract(L.getColumnVector(i - 1));
77 }
78
79 // Return sigma points
80 return sigmaPoints;
81
82 }
83
84 /**
85 * Get the factor applied to the covariance matrix during the unscented transform.
86 * @return the factor applied to the covariance matrix during the unscented transform
87 */
88 protected abstract double getMultiplicationFactor();
89
90 }