Skip to content

J
jfx_kalman_filter_src
Commit 64685669 authored by oscar's avatar oscar
Browse files
Options

speed change is far more than average then throw it

parent 7b73f0e6
Hide whitespace changes
Inline Side-by-side
Showing with 106 additions and 16 deletions
ros2/Track3DEx.cpp
View file @ 64685669
......@@ -45,6 +45,79 @@ void Track3DEx::Update(const std::vector<float>& data)
UpdateDataCheck(data, out);
BaseTrack::UpdateTryConfirm(out);
}
void CalculateAverageAndVar(const std::vector<float>& list, float& average, float& var,int& maxId)
{
float totel = 0;
for(auto iter: list)
totel += iter;
average = totel/list.size();
totel = 0;
int max = -1;
float maxLen = 0;
for(int i = 0; i < list.size();i++)
{
totel += (list[i] - average)*(list[i] - average);
if( abs(list[i] - average) > maxLen)
{
max = i;
maxLen = abs(list[i] - average);
}
}
var = totel/list.size();
maxId = max;
}
void Track3DEx::CalculateAverage(std::vector<float>& listX,std::vector<float>& listY,float& averageX,float& averageY)
{
//求均值和方差
float avgX = 0, var = 0;
int max_x = -1;
CalculateAverageAndVar(listX,avgX,var,max_x);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "CalculateAverageAndVar avgX = %f, var = %f, max_x = %d",avgX,var,max_x);
if(var > m_speedStep && max_x >= 0)
{
int delMax = listX.size()/3 + 1;
int delNum = 0;
while(var > m_speedStep && max_x >= 0 && delNum < delMax)
{
listX.erase(listX.begin() + max_x);
listY.erase(listY.begin() + max_x);
delNum++;
CalculateAverageAndVar(listX,avgX,var,max_x);
}
CalculateAverageAndVar(listX,averageX,var,max_x);
CalculateAverageAndVar(listY,averageY,var,max_x);
return;
}
else
{
float avgY = 0;
CalculateAverageAndVar(listY,avgY,var,max_x);
if(var > m_speedStep && max_x >= 0)
{
int delMax = listY.size()/3 + 1;
int delNum = 0;
while(var > m_speedStep && max_x >= 0 && delNum < delMax)
{
listX.erase(listX.begin() + max_x);
listY.erase(listY.begin() + max_x);
delNum++;
CalculateAverageAndVar(listY,avgY,var,max_x);
}
CalculateAverageAndVar(listX,averageX,var,max_x);
CalculateAverageAndVar(listY,averageY,var,max_x);
return;
}
else
{
averageX = avgX;
averageY = avgY;
}
}
}
bool Track3DEx::IsUpdateConfirm(const std::vector<float>& _x,const std::vector<float>& _x_update)
{
if(_x.size() != 10 || _x_update.size() != 10)
......@@ -55,37 +128,51 @@ bool Track3DEx::IsUpdateConfirm(const std::vector<float>& _x,const std::vector<f
{
m_speedXLists.push_back(_x_update[7]);
m_speedYLists.push_back(_x_update[8]);
// if(m_speedXLists.size() >= 10)
// {
// float avgX,avgY;
// CalculateAverage(m_speedXLists,m_speedYLists,avgX,avgY);
// }
return true;
}
float totelX = 0,totelY = 0;
float totelX_ = 0, totelY_ = 0;
for(int i = 0; i < m_speedXLists.size(); i++)
{
if(i > 0)
{
totelX_ += m_speedXLists[i];
totelY_ += m_speedYLists[i];
}
totelX += m_speedXLists[i];
totelY += m_speedYLists[i];
}
float x_u = _x_update[7];
float y_u = _x_update[8];
float averageX = totelX/m_speedXLists.size();
float averageY = totelY/m_speedYLists.size();
float x_u = _x_update[7];
float y_u = _x_update[8];
if(fabs(averageX - x_u) < m_speedStep*2 && fabs(averageY - y_u) < m_speedStep*2)
float averageX_ = (totelX_ + x_u)/m_speedXLists.size();
float averageY_ = (totelY_ + y_u)/m_speedYLists.size();
if(fabs(averageX - averageX_) < m_speedStep && fabs(averageY - averageY_) < m_speedStep)
{
m_speedXLists.push_back(_x_update[7]);
m_speedYLists.push_back(_x_update[8]);
}
if(m_speedXLists.size() > 10)
m_speedXLists.erase(m_speedXLists.begin());
if(m_speedYLists.size() > 10)
m_speedYLists.erase(m_speedYLists.begin());
if(fabs(averageX - x_u) < m_speedStep && fabs(averageY - y_u) < m_speedStep)
if(m_speedXLists.size() > 10)
m_speedXLists.erase(m_speedXLists.begin());
if(m_speedYLists.size() > 10)
m_speedYLists.erase(m_speedYLists.begin());
return true;
}
else
{
{
if(m_obj)
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "IsUpdateConfirm id = %d, _x = [%f,%f,%f][%f,%f], _x_update = [%f,%f,%f][%f,%f],averageX = %f,averageY = %f",
m_obj->id,_x[0],_x[1],_x[2],_x[7],_x[8],_x_update[0],_x_update[1],_x_update[2],_x_update[7],_x_update[8],averageX,averageY);
return false;
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "IsUpdateConfirm id = %d, _x = [%f,%f,%f][%f,%f], _x_update = [%f,%f,%f][%f,%f],averageX = %f,averageX_ = %f,averageY = %f,averageY_ = %f,m_speedStep = %f,speedX = [%f,%f,%f,%f,%f,%f,%f,%f,%f,%f], speedY = [%f,%f,%f,%f,%f,%f,%f,%f,%f,%f]",
m_obj->id,_x[0],_x[1],_x[2],_x[7],_x[8],_x_update[0],_x_update[1],_x_update[2],_x_update[7],_x_update[8],averageX,averageX_,averageY,averageY_,m_speedStep,
m_speedXLists[0],m_speedXLists[1],m_speedXLists[2],m_speedXLists[3],m_speedXLists[4],m_speedXLists[5],m_speedXLists[6],m_speedXLists[7],m_speedXLists[8],m_speedXLists[9],
m_speedYLists[0],m_speedYLists[1],m_speedYLists[2],m_speedYLists[3],m_speedYLists[4],m_speedYLists[5],m_speedYLists[6],m_speedYLists[7],m_speedYLists[8],m_speedYLists[9]
);
return false;
}
return true;
}
void Track3DEx::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
......@@ -230,8 +317,9 @@ void Track3DEx::GetSpeed(float& x, float& y)//获取速度值
{
if(m_obj->obj)
{
x = m_obj->obj->kinematics.twist_with_covariance.twist.linear.x;
y = m_obj->obj->kinematics.twist_with_covariance.twist.linear.y;
x = m_obj->obj->kinematics.twist_with_covariance.twist.angular.x;
y = m_obj->obj->kinematics.twist_with_covariance.twist.angular.y;
RCLCPP_INFO(rclcpp::get_logger("rclcpp")," GetSpeed = [%f,%f]",x,y);
}
}
}
\ No newline at end of file
ros2/Track3DEx.h
View file @ 64685669
......@@ -23,6 +23,7 @@ public:
float m_speedStep = 0.1f;//速度跳变的最大值
void GetSpeed(float& x, float& y);//获取速度值
void CalculateAverage(std::vector<float>& listX,std::vector<float>& listY,float& averageX,float& averageY);
std::vector<float> m_speedXLists;//记录之前的速度值x
std::vector<float> m_speedYLists;//记录之前的速度值y
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment