113 lines
3.6 KiB
C++
113 lines
3.6 KiB
C++
|
/******************** (C) COPYRIGHT 2023 Geek************************************
|
|||
|
|
* File Name : MagCalibrate.cpp
|
|||
|
|
* Department : Sensor Algorithm Team
|
|||
|
|
* Current Version : V1.0
|
|||
|
|
* Author : zhanli
|
|||
|
|
* Date of Issued : 2023.09.10
|
|||
|
|
* Comments : GeekIMU <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Уģ<EFBFBD><EFBFBD>
|
|||
|
|
********************************************************************************/
|
|||
|
|
/* Header File Including -----------------------------------------------------*/
|
|||
|
|
|
|||
|
|
#include <stdio.h>
|
|||
|
|
#include <math.h>
|
|||
|
|
#include <conio.h>
|
|||
|
|
#include "MagCalibrate.h"
|
|||
|
|
#include <fstream>
|
|||
|
|
|
|||
|
|
MagCaliParam CalculateMagCalibrationParams(MagData _mdata[], int dataSize)
|
|||
|
|
{
|
|||
|
|
MagCaliParam param, lastparam;
|
|||
|
|
|
|||
|
|
srand((unsigned)time(NULL));
|
|||
|
|
|
|||
|
|
float xmin = _mdata[0].x;
|
|||
|
|
float ymin = _mdata[0].y;
|
|||
|
|
float zmin = _mdata[0].z;
|
|||
|
|
float xmax = _mdata[0].x;
|
|||
|
|
float ymax = _mdata[0].y;
|
|||
|
|
float zmax = _mdata[0].z;
|
|||
|
|
float err, errnew;
|
|||
|
|
|
|||
|
|
for (int i = 0; i < dataSize; i++)
|
|||
|
|
{
|
|||
|
|
if (xmax < _mdata[i].x)
|
|||
|
|
xmax = _mdata[i].x;
|
|||
|
|
if (ymax < _mdata[i].y)
|
|||
|
|
ymax = _mdata[i].y;
|
|||
|
|
if (zmax < _mdata[i].z)
|
|||
|
|
zmax = _mdata[i].z;
|
|||
|
|
if (xmin > _mdata[i].x)
|
|||
|
|
xmin = _mdata[i].x;
|
|||
|
|
if (ymin > _mdata[i].y)
|
|||
|
|
ymin = _mdata[i].y;
|
|||
|
|
if (zmin > _mdata[i].z)
|
|||
|
|
zmin = _mdata[i].z;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
lastparam.xOffset = 0.5 * (xmax + xmin);
|
|||
|
|
lastparam.yOffset = 0.5 * (ymax + ymin);
|
|||
|
|
lastparam.zOffset = 0.5 * (zmax + zmin);
|
|||
|
|
lastparam.xScale = 0.5 * abs(xmax - xmin);
|
|||
|
|
lastparam.yScale = 0.5 * abs(ymax - ymin);
|
|||
|
|
lastparam.zScale = 0.5 * abs(zmax - zmin);
|
|||
|
|
err = 0;
|
|||
|
|
for (int i = 0; i < dataSize; i++)
|
|||
|
|
err = err + fabs(pow((_mdata[i].x - lastparam.xOffset), 2) / pow(lastparam.xScale, 2) + pow((_mdata[i].y - lastparam.yOffset), 2) / pow(lastparam.yScale, 2) + pow((_mdata[i].z - lastparam.zOffset), 2) / pow(lastparam.zScale, 2) - 1);
|
|||
|
|
//printf("xc = %f yc = %f zc = %f, a = %f b = %f c= %f,<2C><>ʼ<EFBFBD><CABC>InitErr = %f\n", lastparam.xOffset, lastparam.yOffset, lastparam.zOffset, lastparam.xScale, lastparam.yScale, lastparam.zScale, err);
|
|||
|
|
int starttime = clock();
|
|||
|
|
for (int j = 0; j < 5000; j++)
|
|||
|
|
{
|
|||
|
|
param.xOffset = lastparam.xOffset + rand() / (RAND_MAX + 1.0) - 0.5;
|
|||
|
|
param.yOffset = lastparam.yOffset + rand() / (RAND_MAX + 1.0) - 0.5;
|
|||
|
|
param.zOffset = lastparam.zOffset + rand() / (RAND_MAX + 1.0) - 0.5;
|
|||
|
|
param.xScale = fabs(lastparam.xScale + rand() / (RAND_MAX + 1.0) - 0.5);
|
|||
|
|
param.yScale = fabs(lastparam.yScale + rand() / (RAND_MAX + 1.0) - 0.5);
|
|||
|
|
param.zScale = fabs(lastparam.zScale + rand() / (RAND_MAX + 1.0) - 0.5);
|
|||
|
|
errnew = 0;
|
|||
|
|
for (int i = 0; i < dataSize; i++)
|
|||
|
|
errnew = errnew + fabs(pow((_mdata[i].x - param.xOffset), 2) / pow(param.xScale, 2) + pow((_mdata[i].y - param.yOffset), 2) / pow(param.yScale, 2) + pow((_mdata[i].z - param.zOffset), 2) / pow(param.zScale, 2) - 1);
|
|||
|
|
if (errnew < err)
|
|||
|
|
{
|
|||
|
|
lastparam.xOffset = param.xOffset;
|
|||
|
|
lastparam.yOffset = param.yOffset;
|
|||
|
|
lastparam.zOffset = param.zOffset;
|
|||
|
|
lastparam.xScale = param.xScale;
|
|||
|
|
lastparam.yScale = param.yScale;
|
|||
|
|
lastparam.zScale = param.zScale;
|
|||
|
|
err = errnew;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
param.xScale = 200 / param.xScale;
|
|||
|
|
param.yScale = 200 / param.yScale;
|
|||
|
|
param.zScale = 200 / param.zScale;
|
|||
|
|
int durtime = clock() - starttime;
|
|||
|
|
|
|||
|
|
printf("%f, %f, %f, %f, %f, %f, %f\n cost:%dms\n", param.xOffset, param.yOffset, param.zOffset, param.xScale, param.yScale, param.zScale, err, durtime);
|
|||
|
|
|
|||
|
|
return param;
|
|||
|
|
}
|
|||
|
|
MagCaliParam GetMagDataFormFile()
|
|||
|
|
{
|
|||
|
|
int maxdatasize = 4000;
|
|||
|
|
std::ifstream fin("data.txt");
|
|||
|
|
float x, y, z, x1, y1, z1;
|
|||
|
|
int i = 0;
|
|||
|
|
MagData mdata[4000];
|
|||
|
|
|
|||
|
|
while (fin) {
|
|||
|
|
fin >> x >> y >> z;
|
|||
|
|
fin.get(); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
mdata[i].x = x;
|
|||
|
|
mdata[i].y = y;
|
|||
|
|
mdata[i].z = z;
|
|||
|
|
|
|||
|
|
i++;
|
|||
|
|
|
|||
|
|
if (i == maxdatasize)
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
MagCaliParam p = CalculateMagCalibrationParams(mdata, i);
|
|||
|
|
|
|||
|
|
return p;
|
|||
|
|
}
|