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:
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();
/// Provide an image.
GPU_version/vio/include/gpu_svo/ukf.h
+48 -60
View File
@@ -10,93 +10,87 @@
#include <Eigen/Dense>
class Base{
public:
Base(const Eigen::Matrix<double, 6, 1> &init) {
Base(const Eigen::Matrix<double, 3, 1> &init) {
state_=init;
cov_.setIdentity();
cov_*=0.0001;
};
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){
t_=time;
return;
}
double t=time-t_;
double dt=time-t_;
t_=time;
state_h_<<state_(0)+t*state_(2)+0.5*pow(t,2)*(ddx*cos(state_(4))-ddz*sin(state_(4))),
state_(1)+t*state_(3)+0.5*pow(t,2)*(ddz*cos(state_(4))+ddx*sin(state_(4))),
state_(2)+t*(ddx*cos(state_(4))-ddz*sin(state_(4))),
state_(3)+t*(ddz*cos(state_(4))+ddx*sin(state_(4))),
state_(4)+0.5*t*(state_(5)+dpitch),
dpitch;
Eigen::Matrix<double,6,6> G;
G<<1.0,0,t,0,-0.5*pow(t,2)*(ddz*cos(state_(4))+ddx*sin(state_(4))),0,
0,1.0,0,t,0.5*pow(t,2)*(ddx*cos(state_(4))-ddz*sin(state_(4))),0,
0,0,1.0,0,-t*(ddz*cos(state_(4))+ddx*sin(state_(4))),0,
0,0,0,1.0,t*(ddx*cos(state_(4))-ddz*sin(state_(4))),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();
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2));
state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2));
Eigen::Matrix<double,3,3> R;
R<<svo::Config::Cmd_Cov(),0,0,
0,svo::Config::Cmd_Cov(),0,
0,0,svo::Config::Cmd_Cov();
Eigen::Matrix<double,3,3> G;
G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)),
0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)),
0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
predict_up=true;
}
void correct(double x, double z, double pitch, double time){
Eigen::Matrix<double,3,6> H;
void correct(double ddx, double ddz, double dpitch, double time){
Eigen::Matrix<double,3,3> H;
Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,6,3> k;
Eigen::Matrix<double,3,3> k;
double dt=time-t_;
t_=time;
H<<0,0,1.0,0,0,0,
0,0,0,1.0,0,0,
0,0,0,0,0,1.0;
Q<<svo::Config::Cmd_Cov(),0,0,
0,svo::Config::Cmd_Cov(),0,
0,0,svo::Config::Cmd_Cov();
E<<x-state_h_(2),z-state_h_(3),pitch-state_h_(5);
H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
0,0,1.0;
Q<<svo::Config::ACC_Noise(),0,0,
0,svo::Config::ACC_Noise(),0,
0,0,svo::Config::GYO_Noise();
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();
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;
}
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,1> E;
Eigen::Matrix<double,6,3> k;
Eigen::Matrix<double,3,3> k;
t_=time;
++match;
H<<1.0,0,0,0,0,0,
0,1.0,0,0,0,0,
0,0,0,0,1.0,0;
H<<1.0,0,0,
0,1.0,0,
0,0,1.0;
Q<<svo::Config::Svo_Ekf()/match,0,0,
0,svo::Config::Svo_Ekf()/match,0,
0,0,10*svo::Config::Svo_Ekf()/match;
E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(4);
0,0,50*svo::Config::Svo_Ekf()/match;
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();
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;
}
Eigen::Matrix<double,6,6> cov_;
Eigen::Matrix<double, 6, 1> state_;
Eigen::Matrix<double,3,3> cov_;
Eigen::Matrix<double, 3, 1> state_;
bool predict_up=false;
private:
double t_=0.0;
Eigen::Matrix<double,6,6> cov_h_;
Eigen::Matrix<double, 6, 1> state_h_;//state{x,z,dx,dv,pitch}
Eigen::Matrix<double,3,3> cov_h_;
Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
};
class UKF {
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() {
delete filter_;
@@ -104,30 +98,24 @@ public:
void UpdateIMU(double x,double z,double pitch,const ros::Time& time) {
while(lock)usleep(5);
lock=true;
//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(!filter_->predict_up)filter_->predict(x,z,pitch,1e-9*time.toNSec());
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(filter_->predict_up)filter_->correct(x,z,pitch,1e-9*time.toNSec());
lock=false;
};
void UpdateCmd(double x,double z,double pitch,const ros::Time& time) {
while(lock)usleep(5);
lock=true;
filter_->correct(x,z,pitch,1e-9*time.toNSec());
filter_->predict(x,z,pitch,1e-9*time.toNSec());
lock=false;
};
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);
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;
//std::cout<<"filter cov\n"<<filter_->cov_<<'\n';
//std::cout<<"filter state\n"<<filter_->state_<<'\n';
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);
svo::SE2 out(filter_->state_(2),Eigen::Vector2d(filter_->state_(0),filter_->state_(1)));
return std::pair<Eigen::Matrix<double,3,3>,svo::SE2>(filter_->cov_,out);
};
private:
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
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
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;// +
// 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];
// float chi2 = 0;
Vector3f update; update.setZero();
@@ -337,7 +337,7 @@ bool align2D_SSE2(
float v = cur_px_estimate.y();
// 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 float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
float chi2 = 0;
@@ -502,7 +502,7 @@ bool align2D_NEON (
float v = cur_px_estimate.y();
// 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];
Vector3f update;
Vector3f Jres;
GPU_version/vio/src/frame_handler_mono.cpp
+1 -2
View File
@@ -27,7 +27,7 @@
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(),
cam_(cam),
reprojector_(cam_, map_),
@@ -133,7 +133,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
{
new_frame_->T_f_w_ = last_frame_->T_f_w_;
// sparse image align
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),
vk::getParam<double>("vio/init_ty", 0.0),
vk::getParam<double>("vio/init_tz", 0.0)));
Eigen::Matrix<double,6,1> init;
init<<1e-5,1e-5,1e-5,1e-5,1e-5,1e-5;
Eigen::Matrix<double,3,1> init;
init<<1e-19,1e-19,1e-19;
vo_ = new svo::FrameHandlerMono(cam_,init);
usleep(500);
@@ -157,6 +157,7 @@ void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
}
#endif
vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp;
}
void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};