transform_with_variance.cpp
Go to the documentation of this file.
2 #include <cmath>
3 
4 /*
5  * Takes 2 variances and gives the kalman gain figure
6  * Kalmain gain represents how much to trust the new measurement compared to the existing one
7  * Taken from equation 12 https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
8  */
9 static inline double kalman_gain(double var1, double var2) { return var1 / (var1 + var2); }
10 
11 /*
12  * Returns the probabilty density at a point given the mean and variance of the distribution
13  */
14 static double probabiltyAtPoint(const double x, const double u, const double var) {
15  return (1.0 / (std::sqrt(var) * std::sqrt(2.0 * M_PI))) *
16  std::exp(-((x - u) * (x - u)) / (2.0 * var));
17 }
18 
19 /*
20  * Takes in 2 estimates and the combined estimate and calculates the
21  * variance of the new estimate to normalize it
22  */
23 static double normalizeDavid(const double newMean, const double mean1, double var1,
24  const double mean2, double var2) {
25  // Find the probabilities of the new mean in both original gaussians
26  double prob1_at_newMean = probabiltyAtPoint(newMean, mean1, var1);
27  double prob2_at_newMean = probabiltyAtPoint(newMean, mean2, var2);
28  // We use the sum in quadrature of these 2 probabilities
29  double prob_at_newMean = sqrt(pow(prob1_at_newMean, 2) + pow(prob2_at_newMean, 2));
30 
31  double newVar = std::pow(1.0 / (prob_at_newMean * sqrt(2.0 * M_PI)), 2);
32 
33  // Bound the variance to prevent blow up
34  newVar = std::min(newVar, 1e3);
35  newVar = std::max(newVar, 1e-8);
36 
37  return newVar;
38 }
39 
40 // Update this transform with a new one, with variances as weights
41 // combine variances using David method
42 // If adding a systematic error is desired it should already be added in
44  tf2::Vector3 p1 = transform.getOrigin();
46  double var1 = variance;
47 
48  tf2::Vector3 p2 = newT.transform.getOrigin();
50  double var2 = newT.variance;
51 
52  // Calculate new mean for the position
53  // Use equation 15 in article
54  double k = kalman_gain(var1, var2);
55  transform.setOrigin(p1 + k * (p2 - p1));
56 
57  // Calculate new mean for the orientation
58  // Use equation 15 in article
59  // https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
60  //
61  // The kalman gain should give us the weight for how far towards the new estimate to go
62  // Slerp should put us in a linear frame so the kalman gain should work as is
63  // Kalman gain is always [0,1] ?
65 
66  //
67  // Do the new variance calculations
68  //
69 
70  // Do everything in a 1d space between p1 and p2
71  // This should probably use proper multivariate modeling
72  double mean1 = 0.0;
73  double mean2 = (p2 - p1).length();
74  double mean = (transform.getOrigin() - p1).length();
75 
76  // Normalize the variances so that the area under the probabilty remains 1
77  variance = normalizeDavid(mean, mean1, var1, mean2, var2);
78 }
79 
80 // Weighted average of 2 transforms, variances computed using Alexey Method
82  const TransformWithVariance& t2) {
83  TransformWithVariance out = t1;
84  out.update(t2);
85  return out;
86 
87  /*
88  tf2::Vector3 p1 = t1.transform.getOrigin();
89  tf2::Quaternion o1 = t1.transform.getRotation();
90  double var1 = t1.variance;
91 
92  tf2::Vector3 p2 = t2.transform.getOrigin();
93  tf2::Quaternion o2 = t2.transform.getRotation();
94  double var2 = t2.variance;
95 
96  double k = kalman_gain(var1, var2);
97 
98  out.transform.setOrigin(p1 + k * (p2 - p1));
99  out.transform.setRotation(q1.slerp(q2, k).normalized());
100  out.variance = suminquadrature(var1, var2);
101 
102  return out;
103  */
104 }
Quaternion normalized() const
Quaternion getRotation() const
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
static double probabiltyAtPoint(const double x, const double u, const double var)
static double kalman_gain(double var1, double var2)
Quaternion slerp(const Quaternion &q, const tf2Scalar &t) const
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
static double normalizeDavid(const double newMean, const double mean1, double var1, const double mean2, double var2)
TransformWithVariance averageTransforms(const TransformWithVariance &t1, const TransformWithVariance &t2)
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void update(const TransformWithVariance &newT)


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Tue Jun 1 2021 03:03:29