add kf submodule

Author: Shuo

Diff for: .devcontainer/Dockerfile

@@ -47,6 +47,32 @@ ENV TZ=America/New_York
 RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone
 
 
+# casadi 
+
+ENV SUPPORT_WS=/root/support_files
+WORKDIR $SUPPORT_WS
+RUN git clone https://github.com/casadi/casadi.git
+RUN cd ${SUPPORT_WS}/casadi && git checkout tags/3.5.5
+RUN mkdir ${SUPPORT_WS}/casadi/build && cd ${SUPPORT_WS}/casadi/build
+RUN cmake -DWITH_CPLEX=OFF -DWITH_KNITRO=OFF -DWITH_OOQP=OFF -DWITH_SNOPT=OFF ${SUPPORT_WS}/casadi
+RUN make -j8
+RUN make install
+# /usr/local/lib/casadi/
+# /usr/local/include/casadi
+
+# filter for 
+WORKDIR $SUPPORT_WS
+RUN git clone https://github.com/ShuoYangRobotics/gram_savitzky_golay.git
+WORKDIR ${SUPPORT_WS}/gram_savitzky_golay 
+RUN git submodule init
+RUN git submodule update
+RUN mkdir build 
+WORKDIR ${SUPPORT_WS}/gram_savitzky_golay/build
+RUN cmake -DCMAKE_BUILD_TYPE=Release ../
+RUN make 
+RUN make install
+
+
 ### run docker as a ssh daemon
 RUN apt-get update \
   && apt-get install -y ssh 

Diff for: CMakeLists.txt

@@ -35,6 +35,11 @@ include_directories(
     ${catkin_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIR}
 )
+# search for casadi in the environment
+find_package(casadi REQUIRED) 
+
+# Find the savitzky golay filter package and all its dependencies 
+find_package(gram_savitzky_golay REQUIRED)
 
 catkin_package()
 
@@ -63,10 +68,19 @@ add_library(vilo_lib
     src/factor/imu_leg_factor.cpp src/factor/imu_leg_factor.h src/factor/imu_leg_integration_base.cpp src/factor/imu_leg_integration_base.h
 )
 
+
+# kf subdirectory
+add_subdirectory(src/kalmanFilter)
+
 target_link_libraries(vilo_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
 
 add_executable(vilo src/main.cpp )
-target_link_libraries(vilo vilo_lib)
+target_link_libraries(vilo 
+    vilo_lib
+    kf_lib
+    casadi
+    gram_savitzky_golay::gram_savitzky_golay
+)
 
 
 add_executable(ceres_test src/test/ceres_test.cpp src/legKinematics/A1Kinematics.cpp)

Diff for: src/estimator/estimator.cpp

@@ -152,7 +152,7 @@ void Estimator::setParameter()
     leg_offset_y[0] = 0.047;  leg_offset_y[1] = -0.047;  leg_offset_y[2] = 0.047;   leg_offset_y[3] = -0.047;
     motor_offset[0] = 0.0838; motor_offset[1] = -0.0838; motor_offset[2] = 0.0838;  motor_offset[3] = -0.0838;
     upper_leg_length[0] = upper_leg_length[1] = upper_leg_length[2] = upper_leg_length[3] = 0.21;
-    lower_leg_length[0] = lower_leg_length[1] = lower_leg_length[2] = lower_leg_length[3] = LOWER_LEG_LENGTH;
+    lower_leg_length[0] = lower_leg_length[1] = lower_leg_length[2] = lower_leg_length[3] = VINS_LOWER_LEG_LENGTH;
 
     for (int i = 0; i < NUM_OF_LEG; i++) {
         Eigen::VectorXd rho_fix(RHO_FIX_SIZE); rho_fix << leg_offset_x[i],leg_offset_y[i],motor_offset[i],upper_leg_length[i];
@@ -161,10 +161,10 @@ void Estimator::setParameter()
 
     for (int i = 0; i < WINDOW_SIZE + 1; i++)
     {
-        Rho1[i] = LOWER_LEG_LENGTH*Eigen::Matrix<double, RHO_OPT_SIZE, 1>::Ones();
-        Rho2[i] = LOWER_LEG_LENGTH*Eigen::Matrix<double, RHO_OPT_SIZE, 1>::Ones();
-        Rho3[i] = LOWER_LEG_LENGTH*Eigen::Matrix<double, RHO_OPT_SIZE, 1>::Ones();
-        Rho4[i] = LOWER_LEG_LENGTH*Eigen::Matrix<double, RHO_OPT_SIZE, 1>::Ones();
+        Rho1[i] = VINS_LOWER_LEG_LENGTH*Eigen::Matrix<double, RHO_OPT_SIZE, 1>::Ones();
+        Rho2[i] = VINS_LOWER_LEG_LENGTH*Eigen::Matrix<double, RHO_OPT_SIZE, 1>::Ones();
+        Rho3[i] = VINS_LOWER_LEG_LENGTH*Eigen::Matrix<double, RHO_OPT_SIZE, 1>::Ones();
+        Rho4[i] = VINS_LOWER_LEG_LENGTH*Eigen::Matrix<double, RHO_OPT_SIZE, 1>::Ones();
     }
 
     mProcess.unlock();

Diff for: src/kalmanFilter

@@ -64,7 +64,7 @@ double V_N_TERM2_VAR_RESCALE;
 double V_N_TERM3_DISTANCE_RESCALE;
 double V_N_FINAL_RATIO;
 
-double LOWER_LEG_LENGTH;
+double VINS_LOWER_LEG_LENGTH;
 
 int MAX_CNT;
 int MIN_DIST;
@@ -157,7 +157,7 @@ void readParameters(std::string config_file)
         V_N_TERM3_DISTANCE_RESCALE = fsSettings["v_n_term3_distance_rescale"];
         V_N_FINAL_RATIO = fsSettings["v_n_final_ratio"];
 
-        LOWER_LEG_LENGTH = fsSettings["lower_leg_length"];
+        VINS_LOWER_LEG_LENGTH = fsSettings["lower_leg_length"];
     }
 
     SOLVER_TIME = fsSettings["max_solver_time"];
@@ -167,9 +167,9 @@ void readParameters(std::string config_file)
 
     fsSettings["output_path"] >> OUTPUT_FOLDER;
     if (OPTIMIZE_LEG_BIAS) {
-        VINS_RESULT_PATH = OUTPUT_FOLDER + "/vilo_wb"+ Utility::GetCurrentTimeForFileName() + "-lc-" + to_string(LOWER_LEG_LENGTH) + ".csv";
+        VINS_RESULT_PATH = OUTPUT_FOLDER + "/vilo_wb"+ Utility::GetCurrentTimeForFileName() + "-lc-" + to_string(VINS_LOWER_LEG_LENGTH) + ".csv";
     } else {
-        VINS_RESULT_PATH = OUTPUT_FOLDER + "/vilo_wob"+ Utility::GetCurrentTimeForFileName() + "-lc-" + to_string(LOWER_LEG_LENGTH) + ".csv";
+        VINS_RESULT_PATH = OUTPUT_FOLDER + "/vilo_wob"+ Utility::GetCurrentTimeForFileName() + "-lc-" + to_string(VINS_LOWER_LEG_LENGTH) + ".csv";
     }
     std::cout << "result path " << VINS_RESULT_PATH << std::endl;
     std::ofstream fout(VINS_RESULT_PATH, std::ios::out);

Diff for: src/utils/parameters.cpp

@@ -73,7 +73,7 @@ extern double V_N_TERM2_VAR_RESCALE;
 extern double V_N_TERM3_DISTANCE_RESCALE;
 extern double V_N_FINAL_RATIO;
 
-extern double LOWER_LEG_LENGTH;
+extern double VINS_LOWER_LEG_LENGTH;
 
 extern int MULTIPLE_THREAD;
 // pts_gt for debug purpose;