125 lines
4.2 KiB
C++
125 lines
4.2 KiB
C++
//
|
|
// Created by root on 6/8/21.
|
|
//
|
|
#ifndef GPU_SVO_UKF_H
|
|
#define GPU_SVO_UKF_H
|
|
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include "gpu_svo/config.h"
|
|
#include <Eigen/Dense>
|
|
class Base{
|
|
public:
|
|
Base(const Eigen::Matrix<double, 3, 1> &init) {
|
|
state_=init;
|
|
cov_.setIdentity();
|
|
cov_*=0.0001;
|
|
};
|
|
virtual ~Base(){
|
|
};
|
|
void predict(double dx,double dz,double dpitch,double time){
|
|
if(t_==0.0){
|
|
t_=time;
|
|
return;
|
|
}
|
|
double dt=time-t_;
|
|
t_=time;
|
|
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 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,3,3> k;
|
|
double dt=time-t_;
|
|
t_=time;
|
|
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(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,3> H;
|
|
Eigen::Matrix<double,3,3> Q;
|
|
Eigen::Matrix<double,3,1> E;
|
|
Eigen::Matrix<double,3,3> k;
|
|
t_=time;
|
|
++match;
|
|
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,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(3,3)-k*H)*cov_h_;
|
|
predict_up=false;
|
|
}
|
|
Eigen::Matrix<double,3,3> cov_;
|
|
Eigen::Matrix<double, 3, 1> state_;
|
|
bool predict_up=false;
|
|
private:
|
|
double t_=0.0;
|
|
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, 3, 1>& init) : filter_(new Base(init)) {
|
|
};
|
|
virtual ~UKF() {
|
|
delete filter_;
|
|
};
|
|
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_->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_->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;
|
|
if(filter_->predict_up)filter_->correct(x,z,pitch,match,1e-9*time.toNSec());
|
|
lock=false;
|
|
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;
|
|
bool lock=false;
|
|
};
|
|
#endif //GPU_SVO_UKF_H
|