Write an arithmetic filter to deal with Gaussian random errors when thinking to make the data smoother and more reasonable. Not tall enough, but write it here to improve your work efficiency. A copy of the world's code, the spread of ideas. . . The following uses C#, which is simple and clear, and the principle of the formula is not explained. Like to copy away.
class currentDPointLocClass
{
private float currentpointX;
private float currentpointY;
private float currentpointZ;
private int enNumber;
private float diffx;
private float diffy;
private List<locationClassconfig> currentPtList;
public currentDPointLocClass()
{
currentpointX = 0;
currentpointY = 0;
currentpointZ = 0;
enNumber = 8;
diffx =8.4f;
diffy = 8.4f;
currentPtList = new List<locationClassconfig>();
}
public void addPtPool(float pdx, float pdy ,float pdz)
{
if (Math.Abs(pdx - currentpointX) > diffx || Math.Abs(pdy - currentpointY) > diffy)
{
currentPtList.Clear();
currentpointX = pdx;
currentpointY = pdy;
currentpointZ = pdz;
}
else {
if (currentPtList.Count > enNumber)
{
currentPtList.RemoveRange(0,2);
}
locationClassconfig mlocpt = new locationClassconfig(pdx, pdy, pdz);
currentPtList.Add(mlocpt);
int listsize = currentPtList.Count;
if (listsize > 3)
{
float tmpsize = listsize;
float tmpdf = 1.0f / (tmpsize * 2.0f);
float tmpdc = (2.0f * tmpsize - 1) / (tmpsize * tmpsize - tmpsize);
float sumx = 0;
float sumy = 0;
float sumz = 0;
for (int i = 0; i < listsize; i++)
{
sumx += currentPtList[i].get_locX() * (tmpdf + tmpdc * i) / tmpsize;
sumy += currentPtList[i].get_locY() * (tmpdf + tmpdc * i) / tmpsize;
sumz += currentPtList[i].get_locZ() * (tmpdf + tmpdc * i) / tmpsize;
}
currentpointX = sumx;
currentpointY = sumy;
currentpointZ = sumz;
}
else
{
float sumx = 0;
float sumy = 0;
float sumz = 0;
for (int i = 0; i < listsize; i++)
{
sumx += currentPtList[i].get_locX();
sumy += currentPtList[i].get_locY();
sumz += currentPtList[i].get_locZ();
}
currentpointX = sumx / listsize;
currentpointY = sumy / listsize;
currentpointZ = sumz / listsize;
}
}
}
public float get_currentpointX()
{
return currentpointX;
}
public float get_currentpointY()
{
return currentpointY;
}
public float get_currentpointZ()
{
return currentpointZ;
}
public void set_sumNumber(int num)
{
if (num < 3)
{
enNumber = 3;
}
else {
enNumber = num;
}
}
public void set_diffxy(float dfxy)
{
diffx = dfxy;
diffy = dfxy;
}
public void set_diffx(float dfx)
{
diffx = dfx;
}
public void set_diffy(float dfy)
{
diffy = dfy;
}
}