search

orocos / orocos-bayesian-filtering

The orocos Bayesian Filtering Library
149 stars 59 forks source link

_Mu_new initialize problem #55

Open Needrom opened 1 year ago

Needrom commented 1 year ago

is there some thing wrong with the _Mu_new initialize that i found Pdf<ColumnVector> *posterior = filter->PostGet(); posterior->ExpectedValueGet() here seem not be zero from the beginning.And found the value from posterior->ExpectedValueGet() may come from the class of KalmanFilter from here http://docs.ros.org/en/jade/api/bfl/html/kalmanfilter_8cpp_source.html

Data Showing

here form the data of my terminal output. we can see the input all be zero at the beginning ,but the posterior Mean seem strange posterior Mean[6](-2.18436e+177,1.32349e-23,1e-07,1e-07,1e-07,1e-07)


df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)
posterior Mean[6](-2.18436e+177,1.32349e-23,1e-07,1e-07,1e-07,1e-07)
2.4574 -4.29038 1.82393
24 hours ago, the time was 2023-05-23 11:24:38
Printing took -20524us.
df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)
posterior Mean[6](-2.18392e+177,-0.000858131,4.36784e+171,-8.37904e-06,1e-07,1e-07)
2.51203 -4.18554 1.79193
24 hours ago, the time was 2023-05-23 11:24:38
Printing took -20826us.
df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)
posterior Mean[6](-2.18326e+177,-0.00211415,1.747e+172,-3.33886e-05,8.73306e+169,-6.7377e-08)
2.51203 -4.18554 1.79193
24 hours ago, the time was 2023-05-23 11:24:38
Printing took -20358us.
df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)
posterior Mean[6](-2.18239e+177,-0.00378958,4.36749e+172,-8.35105e-05,3.49156e+170,-5.6919e-07)
2.51203 -4.18554 1.79193
24 hours ago, the time was 2023-05-23 11:24:38
Printing took -19543us.
df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)
posterior Mean[6](-2.1813e+177,-0.00588524,8.73536e+172,-0.000167123,8.72353e+170,-1.57194e-06)
2.51203 -4.18554 1.79193
24 hours ago, the time was 2023-05-23 11:24:38
Printing took -19857us.
df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)
posterior Mean[6](-2.17998e+177,-0.00840226,1.52882e+173,-0.000292611,1.74338e+171,-3.24135e-06)
2.51203 -4.18554 1.79193
24 hours ago, the time was 2023-05-23 11:24:38
Printing took -19519us.
df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)
posterior Mean[6](-2.17845e+177,-0.011342,2.4464e+173,-0.000468372,3.04818e+171,-5.74211e-06)
2.53933 -4.09595 1.78735
24 hours ago, the time was 2023-05-23 11:24:38
Printing took -20385us.
df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)
posterior Mean[6](-2.17669e+177,-0.0146343,3.67015e+173,-0.000697783,4.872e+171,-9.03728e-06)
2.53933 -4.09595 1.78735
24 hours ago, the time was 2023-05-23 11:24:38
Printing took -20670us.
df matrix
[6,6]((1,0,0.02,0,0,0),(0,1,0,0.02,0,0),(0,0,1,0,0.02,0),(0,0,0,1,0,0.02),(0,0,0,0,1,0),(0,0,0,0,0,1))
input: [2](0,0)

and here is the nonlinearanalyticconditionalgaussianmobile.cpp


// $Id: nonlinearanalyticconditionalgaussianmobile.cpp 5823 2005-10-27 13:43:02Z TDeLaet $
// Copyright (C) 2006  Tinne De Laet <first dot last at mech dot kuleuven dot be>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
//

#include "nonlinearanalyticconditionalgaussianmobile_test.h"
#include <wrappers/rng/rng.h> // Wrapper around several rng
                                 // libraries
#define NUMCONDARGUMENTS_MOBILE 2

namespace BFL
{
  using namespace MatrixWrapper;

  NonLinearAnalyticConditionalGaussianMobile::NonLinearAnalyticConditionalGaussianMobile(const Gaussian& additiveNoise)
    : AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE)
  {
  }

  NonLinearAnalyticConditionalGaussianMobile::~NonLinearAnalyticConditionalGaussianMobile(){}

  ColumnVector NonLinearAnalyticConditionalGaussianMobile::ExpectedValueGet() const
  {
    ColumnVector state = ConditionalArgumentGet(0);
    ColumnVector vel  = ConditionalArgumentGet(1);
//    state(1) += cos(state(3)) * vel(1);
//    state(2) += sin(state(3)) * vel(1);
//    state(3) += vel(2);

    state(1) += state(3) * 0.02 + 1/2 * state(5) * pow(0.02, 2);
    state(2) += state(4) * 0.02 + 1/2 * state(6) * pow(0.02, 2);
    state(3) += state(5) * 0.02;
    state(4) += state(6) * 0.02;
    state(5) = vel(1);
    state(6) = vel(2);

    return state + AdditiveNoiseMuGet();
  }

  Matrix NonLinearAnalyticConditionalGaussianMobile::dfGet(unsigned int i) const
  {
    if (i==0)//derivative to the first conditional argument (x)
      {
        ColumnVector state = ConditionalArgumentGet(0);
        ColumnVector vel = ConditionalArgumentGet(1);
    //  Matrix df(3,3);
    //  df(1,1)=1;
    //  df(1,2)=0;
    //  df(1,3)=-vel(1)*sin(state(3));
    //  df(2,1)=0;
    //  df(2,2)=1;
    //  df(2,3)=vel(1)*cos(state(3));
    //  df(3,1)=0;
    //  df(3,2)=0;
    //  df(3,3)=1;

        float dt = 0.02;

        Matrix df(6,6);
        df(1, 1) = 1;
        df(1, 2) = 0;
        df(1, 3) = dt;
        df(1, 4) = 0;
        df(1, 5) = 1/2 * pow(dt, 2);
        df(1, 6) = 0;

        df(2, 1) = 0;
        df(2, 2) = 1;
        df(2, 3) = 0;
        df(2, 4) = dt;
        df(2, 5) = 0;
        df(2, 6) = 1/2 * pow(dt, 2);

        df(3, 1) = 0;
        df(3, 2) = 0;
        df(3, 3) = 1;
        df(3, 4) = 0;
        df(3, 5) = dt;
        df(3, 6) = 0;       

        df(4, 1) = 0;
        df(4, 2) = 0;
        df(4, 3) = 0;
        df(4, 4) = 1;
        df(4, 5) = 0;
        df(4, 6) = dt;

        df(5, 1) = 0;
        df(5, 2) = 0;
        df(5, 3) = 0;
        df(5, 4) = 0;
        df(5, 5) = 1;
        df(6, 6) = 0;

        df(6, 1) = 0;
        df(6, 2) = 0;
        df(6, 3) = 0;
        df(6, 4) = 0;
        df(6, 5) = 0;
        df(6, 6) = 1;

        cout << "df matrix" << endl << df << endl;

        return df;
      }
    else
      {
    if (i >= NumConditionalArgumentsGet())
      {
        cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n";
        exit(-BFL_ERRMISUSE);
      }
    else{
      cerr << "The df is not implemented for the" <<i << "th conditional argument\n";
      exit(-BFL_ERRMISUSE);
    }
      }
  }

}//namespace BFL
toeklk commented 1 year ago

Hi Needrom, without complete code, it is hard to help you out... Did you start from one of the BFL examples? Which 'main' did you use?

Needrom commented 1 year ago

Hi Needrom, without complete code, it is hard to help you out... Did you start from one of the BFL examples? Which 'main' did you use?

yes. of course I start from the EKF example. I change it to the 6 state equation. I can show you the main part of my code.


void SysUncertainty(){
        ColumnVector sys_noise_Mu(6);
        sys_noise_Mu(1) = 0.0000001;
        sys_noise_Mu(2) = 0.0000001;
        sys_noise_Mu(3) = 0.0000001;
        sys_noise_Mu(4) = 0.0000001;
        sys_noise_Mu(5) = 0.0000001;
        sys_noise_Mu(6) = 0.0000001;

        SymmetricMatrix sys_noise_Cov(6);
        sys_noise_Cov = 0.0;
        sys_noise_Cov(1, 1) = 0.0000001;
        sys_noise_Cov(2, 2) = 0.0000001;
        sys_noise_Cov(3, 3) = 0.0000001;
        sys_noise_Cov(4, 4) = 0.0000001;
        sys_noise_Cov(5, 5) = 0.0000001;
        sys_noise_Cov(6, 6) = 0.0000001;

        _system_Uncertainty = new Gaussian(sys_noise_Mu, sys_noise_Cov);
        _sys_pdf = new NonLinearAnalyticConditionalGaussianMobile(*_system_Uncertainty);
        _sys_model = new AnalyticSystemModelGaussianUncertainty(_sys_pdf);
    }

void MeasureUncertainty(){
        ColumnVector meas_noise_Mu(2);
        meas_noise_Mu(1) = 0.001;
        meas_noise_Mu(2) = 0.001;

        SymmetricMatrix meas_noise_Cov(2);
        meas_noise_Cov = 0.0;
        meas_noise_Cov(1, 1) = 0.001;
        meas_noise_Cov(2, 2) = 0.001;

        _measurement_Uncertainty = new Gaussian(meas_noise_Mu, meas_noise_Cov);

        // create matrix _meas_model for linear measurement model
        double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));

        Matrix H(2, 6);
        H = 0.0;
        H(1, 1) = 1;
        H(1, 2) = 0;
        H(1, 3) = 0;
        H(1, 4) = 0;
        H(1, 5) = 0;
        H(1, 6) = 0;

        H(2, 1) = 0;
        H(2, 2) = 1;
        H(2, 3) = 0;
        H(2, 4) = 0;
        H(2, 5) = 0;
        H(2, 6) = 0;

        // create the measurement model
        _meas_pdf = new LinearAnalyticConditionalGaussian(H, *_measurement_Uncertainty);
        _meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(_meas_pdf);
    }

void PriorInitialize(){
        /****************************
         * Linear prior DENSITY     *
         ***************************/
         // Continuous Gaussian prior (for Kalman filters)
        ColumnVector prior_Mu(6);
        prior_Mu = 0.0;

        SymmetricMatrix prior_Cov(6);
        prior_Cov = 0.0;

        Gaussian prior_cont(prior_Mu,prior_Cov);

        /******************************
         * Construction of the Filter *
         ******************************/
        filter = new ExtendedKalmanFilter(&prior_cont);
    }

void topic_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
//      RCLCPP_INFO(this->get_logger(), "I heard: %d %d %d", msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
        static float vx = 0;
        static float vy = 0;
        static bool catch_once = true;
        ColumnVector input(2);  
        input = 0;

        static std::chrono::steady_clock::time_point time_last; 
        std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
    std::time_t now_c = std::chrono::system_clock::to_time_t(now - std::chrono::hours(24));
    std::cout << "24 hours ago, the time was "
              << std::put_time(std::localtime(&now_c), "%F %T") << '\n';

    std::chrono::steady_clock::time_point time_now = std::chrono::steady_clock::now();
    std::cout << "Printing took "
            << std::chrono::duration_cast<std::chrono::microseconds>(time_last - time_now).count()
            << "us.\n";

        eskf_imu_filter_.predict(0.02);
        eskf_imu_filter_.correctGyr(msg->angular_velocity.x, 
                                    msg->angular_velocity.y,
                                    msg->angular_velocity.z);
        eskf_imu_filter_.correctAcc(msg->linear_acceleration.x,
                                    msg->linear_acceleration.y,
                                    msg->linear_acceleration.z);    
        eskf_imu_filter_.reset();

        cout << "ax " << msg->angular_velocity.x << " ay " << msg->angular_velocity.y << " az " << msg->angular_velocity.z << endl;

        IMU_EKF::Quaternion eskf_quat = eskf_imu_filter_.getAttitude();
        float eskf_imu_ax, eskf_imu_ay, eskf_imu_az;
        eskf_imu_filter_.getAcceleration(eskf_imu_ax,
                                        eskf_imu_ay,
                                        eskf_imu_az);

        Eigen::Quaternion eskf_imu_qv_acc((float)0, eskf_imu_ax, eskf_imu_ay, eskf_imu_az);

        Eigen::Quaternion eskf_imu_q(eskf_quat[IMU_EKF::v1],
                                    eskf_quat[IMU_EKF::v2],
                                    eskf_quat[IMU_EKF::v3],
                                    eskf_quat[IMU_EKF::w]); 

        eskf_imu_qv_acc = (eskf_imu_q.inverse()) * eskf_imu_qv_acc * (eskf_imu_q.inverse()).conjugate();    

        float vx_temp, vy_temp, v_norm;

        vx = msg->linear_acceleration.x * 0.02;
        vy = msg->linear_acceleration.y * 0.02;
        input(1) = (float)eskf_imu_qv_acc.x();  
        input(2) = (float)eskf_imu_qv_acc.y();  

        tf2::Vector3 pos = tflisten->getPos();
        ColumnVector meas_state(2);
        meas_state(1) = pos[0];
        meas_state(2) = pos[1];

        tf2::Quaternion q_pose(eskf_imu_q.x(),
                            eskf_imu_q.y(),
                            eskf_imu_q.z(),
                            eskf_imu_q.w());
////        
        ColumnVector res(2);
//      res = _meas_model->Simulate(meas_state);        
//
//////      tfbro->sendTF(meas_ret, q_pose);    
////
        cout << "_state: " << _state << endl;
//      cout << "meas " << res << endl;     
////    
        filter->Update(_sys_model, input, _meas_model, meas_state);

        cout << "measure input state" << meas_state << endl;        

//      filter->Update(_meas_model, meas_state);
//
        Pdf<ColumnVector> *posterior = filter->PostGet();
        cout << "input: " << input << endl;
        cout << "posterior Mean" << posterior->ExpectedValueGet() << endl;
        tf2::Vector3 expected_data;
        expected_data[0] = posterior->ExpectedValueGet()(1);
        expected_data[1] = posterior->ExpectedValueGet()(2);
        expected_data[2] = 0;       

//      // data send back to tf.
        tfbro->sendTF(expected_data, q_pose);

        time_last = time_now; 
    }

just about [x, y, v_x, v_y, a_x, a_y]

Needrom commented 1 year ago

something just happen when I raise the rate to 1000hz . the v_y state just suddenly raise to -2.98118e+130 when the input of a_y is still 0.00297971.