我将传感器放在窗口上,并移动窗口以获得x轴方向的速度和距离。但是当我停止移动时,速度读数不为零,这会导致距离测量错误。
如何获得准确的计算速度?
代码:
static float acceleration[3] = {0.0, 0.0, 0.0};
static float pre_velocity = 0.0;
static float velocity = 0.0;
static float position = 0.0;
//get physical acceleration
LSM6DSO_FIFO_OutRawGet(acceleration);
lsm6dso_from_fs2_to_mg(acceleration);
acceleration[0] = (acceleration[0] /1000) * 9.81f;
acceleration[1] = (acceleration[1] /1000) * 9.81f;
acceleration[2] = (acceleration[2] /1000) * 9.81f;
//low pass filter
acceleration[0] = LPF2pApply_1(acceleration[0]);
acceleration[1] = LPF2pApply_2(acceleration[1]);
acceleration[2] = LPF2pApply_3(acceleration[2]);
//accel mechanical noise filter
if(fabs(acceleration[0]) <= ACC_MECHANICAL_FILTER_TH)
{
acceleration[0] = 0;
}
// Double integration
if(acceleration[0] != 0)
{
velocity = pre_velocity + (acceleration*dt);
position = position + pre_velocity*dt + (0.5*acceleration*dt*dt);
pre_velocity = velocity;
}