public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

edit EKF formula

Browse Source
This commit is contained in:
gfuse
2021-06-30 10:22:49 +02:00
parent fcfcb71f45
commit 9ca9e263ba
6 changed files with 57 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/gpu_svo/frame_handler_mono.h
+1 -1
View File
@@ -33,7 +33,7 @@ class FrameHandlerMono : public FrameHandlerBase
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,6,1>& init); FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init);
virtual ~FrameHandlerMono(); virtual ~FrameHandlerMono();
/// Provide an image. /// Provide an image.
GPU_version/vio/include/gpu_svo/ukf.h
+48 -60
View File
@@ -10,93 +10,87 @@
#include <Eigen/Dense> #include <Eigen/Dense>
class Base{ class Base{
public: public:
Base(const Eigen::Matrix<double, 6, 1> &init) { Base(const Eigen::Matrix<double, 3, 1> &init) {
state_=init; state_=init;
cov_.setIdentity(); cov_.setIdentity();
cov_*=0.0001; cov_*=0.0001;
}; };
virtual ~Base(){ virtual ~Base(){
}; };
void predict(double ddx,double ddz,double dpitch,double time){ void predict(double dx,double dz,double dpitch,double time){
if(t_==0.0){ if(t_==0.0){
t_=time; t_=time;
return; return;
} }
double t=time-t_; double dt=time-t_;
t_=time; t_=time;
state_h_<<state_(0)+t*state_(2)+0.5*pow(t,2)*(ddx*cos(state_(4))-ddz*sin(state_(4))), state_h_(2)=state_(2)+dt*dpitch;
state_(1)+t*state_(3)+0.5*pow(t,2)*(ddz*cos(state_(4))+ddx*sin(state_(4))), state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2));
state_(2)+t*(ddx*cos(state_(4))-ddz*sin(state_(4))), state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2));
state_(3)+t*(ddz*cos(state_(4))+ddx*sin(state_(4))), Eigen::Matrix<double,3,3> R;
state_(4)+0.5*t*(state_(5)+dpitch), R<<svo::Config::Cmd_Cov(),0,0,
dpitch; 0,svo::Config::Cmd_Cov(),0,
Eigen::Matrix<double,6,6> G; 0,0,svo::Config::Cmd_Cov();
G<<1.0,0,t,0,-0.5*pow(t,2)*(ddz*cos(state_(4))+ddx*sin(state_(4))),0, Eigen::Matrix<double,3,3> G;
0,1.0,0,t,0.5*pow(t,2)*(ddx*cos(state_(4))-ddz*sin(state_(4))),0, G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)),
0,0,1.0,0,-t*(ddz*cos(state_(4))+ddx*sin(state_(4))),0, 0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)),
0,0,0,1.0,t*(ddx*cos(state_(4))-ddz*sin(state_(4))),0, 0,0,1.0;
0,0,0,0,1.0,t*dpitch,
0,0,0,0,0,1.0;
Eigen::Matrix<double,6,6> R;
R<<10.0*svo::Config::ACC_Noise(),0,0,0,0,0,
0,10.0*svo::Config::ACC_Noise(),0,0,0,0,
0,0,svo::Config::ACC_Noise(),0,0,0,
0,0,0,svo::Config::ACC_Noise(),0,0,
0,0,0,0,10.0*svo::Config::GYO_Noise(),0,
0,0,0,0,0,svo::Config::GYO_Noise();
cov_h_=G*cov_*G.transpose()+R; cov_h_=G*cov_*G.transpose()+R;
predict_up=true; predict_up=true;
} }
void correct(double x, double z, double pitch, double time){ void correct(double ddx, double ddz, double dpitch, double time){
Eigen::Matrix<double,3,6> H; Eigen::Matrix<double,3,3> H;
Eigen::Matrix<double,3,3> Q; Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E; Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,6,3> k; Eigen::Matrix<double,3,3> k;
double dt=time-t_;
t_=time; t_=time;
H<<0,0,1.0,0,0,0, H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,0,0,1.0,0,0, 0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
0,0,0,0,0,1.0; 0,0,1.0;
Q<<svo::Config::Cmd_Cov(),0,0, Q<<svo::Config::ACC_Noise(),0,0,
0,svo::Config::Cmd_Cov(),0, 0,svo::Config::ACC_Noise(),0,
0,0,svo::Config::Cmd_Cov(); 0,0,svo::Config::GYO_Noise();
E<<x-state_h_(2),z-state_h_(3),pitch-state_h_(5); E(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
E(2)=state_(2)+dt*dpitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E; state_=state_h_+k*E;
cov_=(Eigen::MatrixXd::Identity(6,6)-k*H)*cov_h_; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
predict_up=false; predict_up=false;
} }
void correct(double x, double z,double pitch,size_t match,double time){ void correct(double x, double z,double pitch,size_t match,double time){
Eigen::Matrix<double,3,6> H; Eigen::Matrix<double,3,3> H;
Eigen::Matrix<double,3,3> Q; Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E; Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,6,3> k; Eigen::Matrix<double,3,3> k;
t_=time; t_=time;
++match; ++match;
H<<1.0,0,0,0,0,0, H<<1.0,0,0,
0,1.0,0,0,0,0, 0,1.0,0,
0,0,0,0,1.0,0; 0,0,1.0;
Q<<svo::Config::Svo_Ekf()/match,0,0, Q<<svo::Config::Svo_Ekf()/match,0,0,
0,svo::Config::Svo_Ekf()/match,0, 0,svo::Config::Svo_Ekf()/match,0,
0,0,10*svo::Config::Svo_Ekf()/match; 0,0,50*svo::Config::Svo_Ekf()/match;
E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(4); E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E; state_=state_h_+k*E;
cov_=(Eigen::MatrixXd::Identity(6,6)-k*H)*cov_h_; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
predict_up=false; predict_up=false;
} }
Eigen::Matrix<double,6,6> cov_; Eigen::Matrix<double,3,3> cov_;
Eigen::Matrix<double, 6, 1> state_; Eigen::Matrix<double, 3, 1> state_;
bool predict_up=false; bool predict_up=false;
private: private:
double t_=0.0; double t_=0.0;
Eigen::Matrix<double,6,6> cov_h_; Eigen::Matrix<double,3,3> cov_h_;
Eigen::Matrix<double, 6, 1> state_h_;//state{x,z,dx,dv,pitch} Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
}; };
class UKF { class UKF {
public: public:
UKF(const Eigen::Matrix<double, 6, 1>& init) : filter_(new Base(init)) { UKF(const Eigen::Matrix<double, 3, 1>& init) : filter_(new Base(init)) {
}; };
virtual ~UKF() { virtual ~UKF() {
delete filter_; delete filter_;
@@ -104,30 +98,24 @@ public:
void UpdateIMU(double x,double z,double pitch,const ros::Time& time) { void UpdateIMU(double x,double z,double pitch,const ros::Time& time) {
while(lock)usleep(5); while(lock)usleep(5);
lock=true; lock=true;
//if(abs(x)<1.0)x=pow(x,3);//picked up from your code if(abs(x)<1.0)x=pow(x,3);//picked up from your code
//if(abs(z)<1.0)z=pow(z,3);//picked up from your code if(abs(z)<1.0)z=pow(z,3);//picked up from your code
if(!filter_->predict_up)filter_->predict(x,z,pitch,1e-9*time.toNSec()); if(filter_->predict_up)filter_->correct(x,z,pitch,1e-9*time.toNSec());
lock=false; lock=false;
}; };
void UpdateCmd(double x,double z,double pitch,const ros::Time& time) { void UpdateCmd(double x,double z,double pitch,const ros::Time& time) {
while(lock)usleep(5); while(lock)usleep(5);
lock=true; lock=true;
filter_->correct(x,z,pitch,1e-9*time.toNSec()); filter_->predict(x,z,pitch,1e-9*time.toNSec());
lock=false; lock=false;
}; };
std::pair<Eigen::Matrix<double,3,3>,svo::SE2> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) { std::pair<Eigen::Matrix<double,3,3>,svo::SE2> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
while(lock)usleep(5); while(lock)usleep(5);
lock=true; lock=true;
filter_->correct(-1.0*x,-1.0*z,pitch,match,1e-9*time.toNSec()); if(filter_->predict_up)filter_->correct(x,z,pitch,match,1e-9*time.toNSec());
lock=false; lock=false;
//std::cout<<"filter cov\n"<<filter_->cov_<<'\n'; svo::SE2 out(filter_->state_(2),Eigen::Vector2d(filter_->state_(0),filter_->state_(1)));
//std::cout<<"filter state\n"<<filter_->state_<<'\n'; return std::pair<Eigen::Matrix<double,3,3>,svo::SE2>(filter_->cov_,out);
Eigen::Matrix<double,3,3> cov;
cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,4),
filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,4),
filter_->cov_(4,0),filter_->cov_(4,1),filter_->cov_(4,4);
svo::SE2 out(filter_->state_(4),Eigen::Vector2d(-1.0*filter_->state_(0),-1.0*filter_->state_(1)));
return std::pair<Eigen::Matrix<double,3,3>,svo::SE2>(cov,out);
}; };
private: private:
Base* filter_= nullptr; Base* filter_= nullptr;
GPU_version/vio/param/vo_fast.yaml
+1 -1
View File
@@ -12,4 +12,4 @@ svo:
ACC_noise: 0.1 #acc white noise in continuous in EKF ACC_noise: 0.1 #acc white noise in continuous in EKF
GYO_noise: 0.1 #gyro white noise in continuous in EKF GYO_noise: 0.1 #gyro white noise in continuous in EKF
svo_ekf: 5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF svo_ekf: 5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.05 #control vector noise in EKF Cmd_Cov: 0.5 #control vector noise in EKF
GPU_version/vio/src/feature_alignment.cpp
+3 -3
View File
@@ -203,7 +203,7 @@ bool align2D(
bool sign=true;// + bool sign=true;// +
// termination condition // termination condition
const float min_update_squared = 0.1;//0.001 const float min_update_squared = 0.5;//0.001
const int cur_step = cur_img.step.p[0]; const int cur_step = cur_img.step.p[0];
// float chi2 = 0; // float chi2 = 0;
Vector3f update; update.setZero(); Vector3f update; update.setZero();
@@ -337,7 +337,7 @@ bool align2D_SSE2(
float v = cur_px_estimate.y(); float v = cur_px_estimate.y();
// termination condition // termination condition
const float min_update_squared = 0.03*0.03; const float min_update_squared = 0.1*0.1;
const int cur_step = cur_img.step.p[0]; const int cur_step = cur_img.step.p[0];
const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16 const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
float chi2 = 0; float chi2 = 0;
@@ -502,7 +502,7 @@ bool align2D_NEON (
float v = cur_px_estimate.y(); float v = cur_px_estimate.y();
// termination condition // termination condition
const float min_update_squared = 0.03*0.03; const float min_update_squared = 0.1*0.1;
const int cur_step = cur_img.step.p[0]; const int cur_step = cur_img.step.p[0];
Vector3f update; Vector3f update;
Vector3f Jres; Vector3f Jres;
GPU_version/vio/src/frame_handler_mono.cpp
+1 -2
View File
@@ -27,7 +27,7 @@
namespace svo { namespace svo {
FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,6,1>& init) : FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init) :
FrameHandlerBase(), FrameHandlerBase(),
cam_(cam), cam_(cam),
reprojector_(cam_, map_), reprojector_(cam_, map_),
@@ -133,7 +133,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
{ {
new_frame_->T_f_w_ = last_frame_->T_f_w_; new_frame_->T_f_w_ = last_frame_->T_f_w_;
// sparse image align // sparse image align
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
GPU_version/vio/src/vo_node.cpp
+3 -2
View File
@@ -74,8 +74,8 @@ VoNode::VoNode() :
Eigen::Vector3d(vk::getParam<double>("vio/init_tx", 0.0), Eigen::Vector3d(vk::getParam<double>("vio/init_tx", 0.0),
vk::getParam<double>("vio/init_ty", 0.0), vk::getParam<double>("vio/init_ty", 0.0),
vk::getParam<double>("vio/init_tz", 0.0))); vk::getParam<double>("vio/init_tz", 0.0)));
Eigen::Matrix<double,6,1> init; Eigen::Matrix<double,3,1> init;
init<<1e-5,1e-5,1e-5,1e-5,1e-5,1e-5; init<<1e-19,1e-19,1e-19;
vo_ = new svo::FrameHandlerMono(cam_,init); vo_ = new svo::FrameHandlerMono(cam_,init);
usleep(500); usleep(500);
@@ -157,6 +157,7 @@ void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
} }
#endif #endif
vo_->UpdateIMU(imu_in,imu->header.stamp); vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp;
} }
void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) { void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z}; double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};