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
18 package org.hipparchus.util;
19
20 import org.hipparchus.linear.ArrayRealVector;
21 import org.hipparchus.linear.RealVector;
22
23 /**
24 * Unscented transform as defined by Merwe and Wan.
25 * <p>
26 * The unscented transform uses three parameters: alpha, beta and kappa.
27 * Alpha determines the spread of the sigma points around the process state,
28 * kappa is a secondary scaling parameter, and beta is used to incorporate
29 * prior knowledge of the distribution of the process state.
30 * </p>
31 * @see "E. A. Wan and R. Van der Merwe, The unscented Kalman filter for nonlinear estimation,
32 * in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000."
33 * @since 2.2
34 */
35 public class MerweUnscentedTransform extends AbstractUnscentedTransform {
36
37 /** Default value for alpha (0.5, see reference). */
38 public static final double DEFAULT_ALPHA = 0.5;
39
40 /** Default value for beta (2.0, see reference). */
41 public static final double DEFAULT_BETA = 2.0;
42
43 /** Default value for kappa, (0.0, see reference). */
44 public static final double DEFAULT_KAPPA = 0.0;
45
46 /** Weights for covariance matrix. */
47 private final RealVector wc;
48
49 /** Weights for mean state. */
50 private final RealVector wm;
51
52 /** Factor applied to the covariance matrix during the unscented transform (lambda + process state size). */
53 private final double factor;
54
55 /**
56 * Default constructor.
57 * <p>
58 * This constructor uses default values for alpha, beta, and kappa.
59 * </p>
60 * @param stateDim the dimension of the state
61 * @see #DEFAULT_ALPHA
62 * @see #DEFAULT_BETA
63 * @see #DEFAULT_KAPPA
64 * @see #MerweUnscentedTransform(int, double, double, double)
65 */
66 public MerweUnscentedTransform(final int stateDim) {
67 this(stateDim, DEFAULT_ALPHA, DEFAULT_BETA, DEFAULT_KAPPA);
68 }
69
70 /**
71 * Simple constructor.
72 * @param stateDim the dimension of the state
73 * @param alpha scaling control parameter
74 * (determines the spread of the sigma points around the process state)
75 * @param beta free parameter
76 * (used to incorporate prior knowledge of the distribution of the process state)
77 * @param kappa secondary scaling factor
78 * (usually set to 0.0)
79 */
80 public MerweUnscentedTransform(final int stateDim, final double alpha,
81 final double beta, final double kappa) {
82
83 // Call super constructor
84 super(stateDim);
85
86 // lambda = alpha² + (n + kappa) - n (see Eq. 15)
87 final double lambda = alpha * alpha * (stateDim + kappa) - stateDim;
88
89 // Initialize multiplication factor for covariance matrix
90 this.factor = stateDim + lambda;
91
92 // Initialize vectors weights
93 wm = new ArrayRealVector(2 * stateDim + 1);
94 wc = new ArrayRealVector(2 * stateDim + 1);
95
96 // Computation of unscented kalman filter weights (See Eq. 15)
97 wm.setEntry(0, lambda / factor);
98 wc.setEntry(0, lambda / factor + (1.0 - alpha * alpha + beta));
99 final double w = 1.0 / (2.0 * factor);
100 for (int i = 1; i <= 2 * stateDim; i++) {
101 wm.setEntry(i, w);
102 wc.setEntry(i, w);
103 }
104
105 }
106
107 /** {@inheritDoc} */
108 @Override
109 public RealVector getWc() {
110 return wc;
111 }
112
113 /** {@inheritDoc} */
114 @Override
115 public RealVector getWm() {
116 return wm;
117 }
118
119 /** {@inheritDoc} */
120 @Override
121 protected double getMultiplicationFactor() {
122 return factor;
123 }
124
125 }