/******************************************************************************
/ 函数功能:声呐数据处理
/ 修改日期:none
/ 输入参数:none
/ 输出参数:none
/ 使用说明:none
******************************************************************************/
void SonarDataProcess() //T=60ms
{
u8 i;
u16 temp_h2=0;//,SonarHight_avr;//,
static u8 filpoint_h = 0,cnt = 0,Nospeedcnt = 0,contincnt = 0; //滤波器指针
SonarHight_Raw = SR04_HightCal();
//--------------------------简单的去掉极值点------------------------------------
FiltArray_H[filpoint_h] = SonarHight_Raw; //每采集一个，放入队列中
for(i=0; i<FILTERZISE_HIGH; i++)
{
temp_h2 += FiltArray_H[i];
}
SonarHight_avr = temp_h2/FILTERZISE_HIGH; //求和求平均
filpoint_h++;
if(filpoint_h==FILTERZISE_HIGH) filpoint_h =0;
if(SonarHight_Raw<SonarHight_avr>>1) SonarHight_Raw = SonarHight_Last;//若测量值小之前平均值的一半则等于上一次
else if(SonarHight_Raw>(SonarHight_avr>>1)*3) SonarHight_Raw = SonarHight_Last;//等于上
//--------------------------------------------------------------
SonarHight = KalmanFilter(SonarHight_Raw);
SonarHightvel = (SonarHight - SonarHight_Last)*10; //乘以10变为mm/s
SonarHight_Last = SonarHight;
}
ObjectTracker::ObjectTracker(const ObjectMatch& match)
: m_nframes_without_detection(0),
m_nframes_with_detection(0),
m_has_updated_pose(false)
{
m_last_pose = match.pose()->pose3d();
m_raw_pose = match.pose()->pose3d();
m_estimated_pose = m_last_pose;
m_model = &(match.model());
m_last_projected_bounding_rect = bounding_box(match.pose()->projectedBoundingRect());
/*
* 12 variables
* tx, Dtx, ty, Dy, tz, Dtz, rx, Drx, ry, Dry, rz, Drz
* with D the derivative.
* This tracking assumes a constant speed model.
* Rotation are given as axis / angle as magnitude
* representation.
*/
m_kalman = KalmanFilter(12, 12, 0);
Mat1f state = Mat(12, 1, CV_32FC1);
Mat1f process_noise (12, 1);
Mat1f measurement (12, 1);
Mat1f measurement_noise (12, 1);
measurement = 0.f;
/*
* If there were only two variables, this generates a matrix
* with the following form:
* 1 1 0 0
* 0 1 0 0
* 0 0 1 1
* 0 0 1 0
*/
m_kalman.transitionMatrix = Mat1f(12, 12);
setIdentity(m_kalman.transitionMatrix);
for (int i = 0; i < 12; i += 2)
{
m_kalman.transitionMatrix.at<float>(i,i+1) = 1;
}
setIdentity(m_kalman.measurementMatrix, 1);
setIdentity(m_kalman.processNoiseCov, 1e-5);
setIdentity(m_kalman.measurementNoiseCov, 1e-1);
setIdentity(m_kalman.errorCovPost, 1);
cv::Vec3f r = m_last_pose.cvRodriguesRotation();
cv::Vec3f t = m_last_pose.cvTranslation();
// derivatives are null initially.
((Mat1f)m_kalman.statePost) << t[0], 0, t[1], 0, t[2], 0, r[0], 0, r[1], 0, r[2], 0;
}
void Learning::initKalmanFilter(){
tFilter = KalmanFilter(6,3,0);
tFilter.statePre = Mat::zeros(6,1,CV_32FC1);
//setIdentity(kFilter.transitionMatrix);
tFilter.transitionMatrix = *(Mat_<float>(6,6) <<1,0,0,1,0,0, 0,1,0,0,1,0, 0,0,1,0,0,1, 0,0,0,1,0,0, 0,0,0,0,1,0, 0,0,0,0,0,1);
setIdentity(tFilter.measurementMatrix);
setIdentity(tFilter.processNoiseCov, Scalar::all(0.1));
setIdentity(tFilter.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(tFilter.errorCovPost, Scalar::all(0.1));
//init the quaternion kalmanfilter
qFilter = KalmanFilter(3,3,0);
qFilter.statePre = Mat::zeros(3,1,CV_32FC1);
setIdentity(qFilter.transitionMatrix);
setIdentity(qFilter.measurementMatrix);
setIdentity(qFilter.processNoiseCov, Scalar::all(0.1));
setIdentity(qFilter.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(qFilter.errorCovPost, Scalar::all(0.1));
}
int main (int argc, char **argv){
int const accSigma = 1;
int const t = (argc == 2)?(lexical_cast<int>(argv[1])):(4);
double const deltaT = 1.0;
dmat A(2,2), B(2,2), G(2,1), Q(2,2);
dmat State(2,1), Cov(2,2), Ctrl(2,1);
std::vector<dmat> Result;
A(0,0) = 1;A(0,1) = deltaT;
A(1,0) = 0;A(1,1) = 1;
B(0,0) = 0;B(0,1) = 0;
B(1,0) = 0;B(1,1) = 0;
G(0,0) = deltaT * deltaT * 0.5;
G(1,0) = deltaT;
Q = accSigma * prod(G, trans(G));
State(0,0) = 0;
State(1,0) = 0;
Cov(0,0) = 1;Cov(0,1) = 0;
Cov(1,0) = 0;Cov(1,1) = 1;
Ctrl(0,0) = 0;
Ctrl(1,0) = 0;
KalmanFilter kf = KalmanFilter(&A, &B, &Q, accSigma);
for(int i = 0; i < t; ++i){
string fileName = "KFCorrect(k=" + lexical_cast<string>(i + 1) + ").txt";
ofstream ofs(fileName.c_str());
Result = kf.Correct(&State, &Cov, &Ctrl, &G);
State = Result.at(0);
Cov = Result.at(1);
for(double location = -20.0; location <= 20.0; location += 0.5){
for(double vel = -10.0; vel <= 10.0; vel += 0.5){
dmat Info(2,1);
Info(0,0) = location;
Info(1,0) = vel;
double prob = kf.calcBelief(&Info, &State, &Cov);
ofs << location << "\t" << vel << "\t" << prob << endl;
}
}
}
return 0;
}
Kalman::Kalman()
{
//For Kalman filtter
kman.KF = KalmanFilter(4, 2, 0);
kman.state = Mat_<float> (4, 1);
kman.KF.transitionMatrix = *(cv::Mat_<float>(4, 4) << 1,0,1,0,
0,1,0,1, 0,0,1,0, 0,0,0,1);
setIdentity(kman.KF.measurementMatrix, cv::Scalar::all(1));
setIdentity(kman.KF.processNoiseCov, cv::Scalar::all(1e-0));
setIdentity(kman.KF.measurementNoiseCov, cv::Scalar::all(0.1));
setIdentity(kman.KF.errorCovPost, cv::Scalar::all(.1));
randn(kman.KF.statePost, cv::Scalar::all(0), cv::Scalar::all(0.1));
//End
}
KalmanFilter2D::KalmanFilter2D()
{
filter = KalmanFilter(8,4);
//setup error covariances
setIdentity(filter.processNoiseCov, Scalar::all(1e-4));
setIdentity(filter.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(filter.errorCovPost, Scalar::all(.1));
//setup transition matrix with F
filter.transitionMatrix = (Mat_<float>(8,8) << F );
//setup measurement matrix with H
filter.measurementMatrix = (Mat_<float>(4,8) << H);
measurement = Mat_<float>(4,1);
}
void targetCallBack(const laser_package::state::ConstPtr& msg)
{
rho = msg->Measured_Rho;
theta = msg->Measured_Theta;
real_state(XI_INDEX) = msg->Real_X;
real_state(XI_DOT_INDEX) = msg->Real_X_Speed;
real_state(ETA_INDEX) = msg->Real_Y;
real_state(ETA_DOT_INDEX) = msg->Real_Y_Speed;
real_state(OMEGA_INDEX) = msg->Real_Omega;
z(RHO_INDEX) = rho;
z(THETA_INDEX) = theta;
if(msg_count>1)
{
update_time = msg->Time_Of_Measurement;
//extended kalman filter
ExtendedKF.updateFilter(z, update_time,real_state);
ExtendedKF.updateCovariance();
updateStateMessage(&ExtendedKF,msg);
extended_kalman_pub.publish(state_msg);
//regular kalman filter
KF.updateFilter(z, update_time, real_state);
KF.updateCovariance();
updateStateMessage(&KF,msg);
kalman_pub.publish(state_msg);
//imm
imm.update();
updateStateMessage(&imm,msg);
imm_pub.publish(state_msg);
}
else if(msg_count == 0)
{
first_xi = rho*cos(theta)+SENSOR_XI;
first_eta = rho*sin(theta)+SENSOR_ETA;
first_time = msg->Time_Of_Measurement;
msg_count++;
}
else if(msg_count==1)
{
second_time = msg->Time_Of_Measurement;
T = SAMPLING_INTERVAL;
initial_state(XI_INDEX) = rho*cos(theta)+SENSOR_XI;
initial_state(ETA_INDEX) = rho*sin(theta)+SENSOR_ETA;
initial_state(XI_DOT_INDEX) = (initial_state(XI_INDEX)-first_xi)/(T);
initial_state(ETA_DOT_INDEX) = (initial_state(ETA_INDEX)-first_eta)/(T);
initial_state(OMEGA_INDEX) = 0.0001;
//extended kalman filter
ExtendedKF = ExtendedKalmanFilter(initial_state,T , extended_kalman_noise_data,0.5,z);//instantiate Extended kalman filter
updateStateMessage(&ExtendedKF,msg);
extended_kalman_pub.publish(state_msg);
//regular kalman filter
KF = KalmanFilter(initial_state,T , kalman_noise_data, 0.5,z);//instantiate kalman filter
updateStateMessage(&KF,msg);
kalman_pub.publish(state_msg);
//imm
filters.push_back(&KF);
filters.push_back(&ExtendedKF);
imm = IMM(filters);//instantiate IMM with vector of filters
imm.update();
msg_count++;
}
//These are for when we want to publish to Rviz
target_point_msg.x = msg->Real_X;
target_point_msg.y = msg->Real_Y;
target_point_stamped_msg.point = target_point_msg;
target_point_stamped_msg.header.frame_id = "/my_frame";
//The below allows us to publish values so that Rviz can plot. You decide which points to plot from the target class.
target_real_pub.publish(target_point_stamped_msg);
}
