Changing default alpha parameter and adding error

cra-ros-pkg · Oct 26, 2022 · d6a85fd · d6a85fd
1 parent 710d68e
commit d6a85fd
Show file tree

Hide file tree

Showing 9 changed files with 44 additions and 10 deletions.
diff --git a/src/ukf.cpp b/src/ukf.cpp
@@ -62,12 +62,12 @@ namespace RobotLocalization
     covarWeights_.resize(sigmaCount);
 
     stateWeights_[0] = lambda_ / (STATE_SIZE + lambda_);
-    covarWeights_[0] = stateWeights_[0] + (1 - (alpha * alpha) + beta);
+    covarWeights_[0] = stateWeights_[0] + (1.0 - (alpha * alpha) + beta);
     sigmaPoints_[0].setZero();
     for (size_t i = 1; i < sigmaCount; ++i)
     {
       sigmaPoints_[i].setZero();
-      stateWeights_[i] =  1 / (2 * (STATE_SIZE + lambda_));
+      stateWeights_[i] = 1.0 / (2.0 * (STATE_SIZE + lambda_));
       covarWeights_[i] = stateWeights_[i];
     }
   }

diff --git a/src/ukf_localization_node.cpp b/src/ukf_localization_node.cpp
@@ -30,25 +30,43 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include "robot_localization/filter_common.h"
 #include "robot_localization/ros_filter_types.h"
 
 #include <ros/ros.h>
 
+#include <cmath>
 #include <cstdlib>
 #include <vector>
 
 int main(int argc, char **argv)
 {
+  using RobotLocalization::STATE_SIZE;
+
   ros::init(argc, argv, "ukf_navigation_node");
   ros::NodeHandle nh;
   ros::NodeHandle nhLocal("~");
 
   std::vector<double> args(3, 0);
 
-  nhLocal.param("alpha", args[0], 0.001);
+  nhLocal.param("alpha", args[0], ::sqrt(2.0));
   nhLocal.param("kappa", args[1], 0.0);
   nhLocal.param("beta",  args[2], 2.0);
 
+  const auto lambda = args[0] * args[0] * (STATE_SIZE + args[1]) - STATE_SIZE;
+  if (lambda < 0.0)
+  {
+    // Provide suggestions for getting a lambda value of 1.0, which would yield
+    // a weight of 0.0625 for the first sigma point and 0.03125 for the others.
+    const auto suggested_alpha = ::sqrt((STATE_SIZE * 2.0) / (STATE_SIZE + args[1]));
+    const auto suggested_kappa = ((2.0 * STATE_SIZE) / (args[0] * args[0])) - STATE_SIZE;
+
+    ROS_ERROR_STREAM("Invalid UKF parameters specified! Lambda value would be " << lambda <<
+      ". Please increase the value of the alpha parameter to at least " << std::setprecision(10) <<
+      suggested_alpha << " OR increase the kappa parameter to at least " << suggested_kappa <<
+      ". Filter will likely behave erratically.");
+  }
+
   RobotLocalization::RosUkf ukf(nh, nhLocal, args);
   ukf.initialize();
   ros::spin();

diff --git a/src/ukf_localization_nodelet.cpp b/src/ukf_localization_nodelet.cpp
@@ -50,17 +50,33 @@ class UkfNodelet : public nodelet::Nodelet
 public:
   virtual void onInit()
   {
+    using RobotLocalization::STATE_SIZE;
+
     NODELET_DEBUG("Initializing nodelet...");
 
     ros::NodeHandle nh      = getNodeHandle();
     ros::NodeHandle nh_priv = getPrivateNodeHandle();
 
     std::vector<double> args(3, 0);
 
-    nh_priv.param("alpha", args[0], 0.001);
+    nh_priv.param("alpha", args[0], ::sqrt(2.0));
     nh_priv.param("kappa", args[1], 0.0);
     nh_priv.param("beta",  args[2], 2.0);
 
+    const auto lambda = args[0] * args[0] * (STATE_SIZE + args[1]) - STATE_SIZE;
+    if (lambda < 0.0)
+    {
+      // Provide suggestions for getting a lambda value of 1.0, which would yield
+      // a weight of 0.0625 for the first sigma point and 0.03125 for the others.
+      const auto suggested_alpha = ::sqrt((STATE_SIZE + 1.0) / (STATE_SIZE + args[1]));
+      const auto suggested_kappa = ((STATE_SIZE + 1.0) / (args[0] * args[0])) - STATE_SIZE;
+
+      ROS_ERROR_STREAM("Invalid UKF parameters specified! Lambda value would be " << lambda <<
+        ". Please increase the value of the alpha parameter to at least " << std::setprecision(10) <<
+        suggested_alpha << " OR increase the kappa parameter to at least " << suggested_kappa <<
+        ". Filter will likely behave erratically.");
+    }
+
     ukf = std::make_unique<RosUkf>(nh, nh_priv, getName(), args);
     ukf->initialize();
   }

diff --git a/test/test_ukf.cpp b/test/test_ukf.cpp
@@ -57,7 +57,7 @@ class RosUkfPassThrough : public RosUkf
 TEST(UkfTest, Measurements)
 {
   std::vector<double> args;
-  args.push_back(0.001);
+  args.push_back(::sqrt(2.0));
   args.push_back(0);
   args.push_back(2);
 

diff --git a/test/test_ukf_localization_node_bag1.test b/test/test_ukf_localization_node_bag1.test
@@ -92,7 +92,7 @@
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>
 
-      <param name="alpha" value="0.001"/> 
+      <param name="alpha" value="1.414213562"/>
       <param name="kappa" value="0"/> 
       <param name="beta" value="2"/>
 

diff --git a/test/test_ukf_localization_node_bag2.test b/test/test_ukf_localization_node_bag2.test
@@ -73,7 +73,7 @@
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>
 
-      <param name="alpha" value="0.001"/> 
+      <param name="alpha" value="1.414213562"/>
       <param name="kappa" value="0"/> 
       <param name="beta" value="2"/>
 

diff --git a/test/test_ukf_localization_node_bag3.test b/test/test_ukf_localization_node_bag3.test
@@ -45,7 +45,7 @@
 
       <param name="imu0_remove_gravitational_acceleration" value="true"/>
 
-      <param name="alpha" value="0.001"/> 
+      <param name="alpha" value="1.414213562"/>
       <param name="kappa" value="0"/> 
       <param name="beta" value="2"/>
 

diff --git a/test/test_ukf_localization_node_interfaces.test b/test/test_ukf_localization_node_interfaces.test
@@ -62,7 +62,7 @@
                                                   0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
                                                   0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
 
-      <param name="alpha" value="0.001"/>
+      <param name="alpha" value="1.414213562"/>
       <param name="kappa" value="0"/>
       <param name="beta" value="2"/>
 

diff --git a/test/test_ukf_localization_nodelet_bag1.test b/test/test_ukf_localization_nodelet_bag1.test
@@ -93,7 +93,7 @@
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>
 
-      <param name="alpha" value="0.001"/> 
+      <param name="alpha" value="1.414213562"/>
       <param name="kappa" value="0"/> 
       <param name="beta" value="2"/>