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.linear.ArrayRealVector;
20 import org.hipparchus.linear.MatrixUtils;
21 import org.hipparchus.linear.RealMatrix;
22 import org.hipparchus.linear.RealVector;
23
24 /**
25 * Provider for unscented transform.
26 * @since 2.2
27 */
28 public interface UnscentedTransformProvider {
29
30 /**
31 * Perform the unscented transform from a state and its covariance.
32 * @param state process state
33 * @param covariance covariance associated with the process state
34 * @return an array containing the sigma points of the unscented transform
35 */
36 RealVector[] unscentedTransform(RealVector state, RealMatrix covariance);
37
38 /**
39 * Computes a weighted mean state from a given set of sigma points.
40 * <p>
41 * This method can be used for computing both the mean state and the mean measurement
42 * in an Unscented Kalman filter.
43 * </p>
44 * <p>
45 * It corresponds to Equation 17 of "Wan, E. A., & Van Der Merwe, R. The unscented Kalman filter for nonlinear estimation"
46 * </p>
47 * @param sigmaPoints input samples
48 * @return weighted mean state
49 */
50 default RealVector getUnscentedMeanState(RealVector[] sigmaPoints) {
51
52 // Sigma point dimension
53 final int sigmaPointDimension = sigmaPoints[0].getDimension();
54
55 // Compute weighted mean
56 // ---------------------
57
58 RealVector weightedMean = new ArrayRealVector(sigmaPointDimension);
59
60 // Compute the weight coefficients wm
61 final RealVector wm = getWm();
62
63 // Weight each sigma point and sum them
64 for (int i = 0; i < sigmaPoints.length; i++) {
65 weightedMean = weightedMean.add(sigmaPoints[i].mapMultiply(wm.getEntry(i)));
66 }
67
68 return weightedMean;
69 }
70
71 /** Computes the unscented covariance matrix from a weighted mean state and a set of sigma points.
72 * <p>
73 * This method can be used for computing both the predicted state
74 * covariance matrix and the innovation covariance matrix in an Unscented Kalman filter.
75 * </p>
76 * <p>
77 * It corresponds to Equation 18 of "Wan, E. A., & Van Der Merwe, R. The unscented Kalman filter for nonlinear estimation"
78 * </p>
79 * @param sigmaPoints input sigma points
80 * @param meanState weighted mean state
81 * @return the unscented covariance matrix
82 */
83 default RealMatrix getUnscentedCovariance(RealVector[] sigmaPoints, RealVector meanState) {
84
85 // State dimension
86 final int stateDimension = meanState.getDimension();
87
88 // Compute covariance matrix
89 // -------------------------
90
91 RealMatrix covarianceMatrix = MatrixUtils.createRealMatrix(stateDimension, stateDimension);
92
93 // Compute the weight coefficients wc
94 final RealVector wc = getWc();
95
96 // Reconstruct the covariance
97 for (int i = 0; i < sigmaPoints.length; i++) {
98 final RealMatrix diff = MatrixUtils.createColumnRealMatrix(sigmaPoints[i].subtract(meanState).toArray());
99 covarianceMatrix = covarianceMatrix.add(diff.multiplyTransposed(diff).scalarMultiply(wc.getEntry(i)));
100 }
101
102 return covarianceMatrix;
103 }
104
105 /**
106 * Perform the inverse unscented transform from an array of sigma points.
107 * @param sigmaPoints array containing the sigma points of the unscented transform
108 * @return mean state and associated covariance
109 */
110 default Pair<RealVector, RealMatrix> inverseUnscentedTransform(RealVector[] sigmaPoints) {
111
112 // Mean state
113 final RealVector meanState = getUnscentedMeanState(sigmaPoints);
114
115 // Return state and covariance
116 return new Pair<>(meanState, getUnscentedCovariance(sigmaPoints, meanState));
117 }
118
119 /**
120 * Get the covariance weights.
121 * @return the covariance weights
122 */
123 RealVector getWc();
124
125 /**
126 * Get the mean weights.
127 * @return the mean weights
128 */
129 RealVector getWm();
130
131 }