_Mu_new initialize problem

[Needrom]

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]

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]

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]

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.