C++ 地磁校准

地磁标定校准

#include <iostream>
#include <vector>
#include <algorithm>
#include <fstream>
#include <sstream>
#include <ctime>

using namespace std;

bool load_csv(vector<float> &data_x, vector<float> &data_y, vector<float> &data_z){
    string string1 = "data.csv";
    ifstream ifs(string1, ios::in);
    if (!ifs.is_open())
    {
        cerr << "open file failed!" << endl;
        exit(-1);
    }

    string _line;
    vector<string> subArray;

    while (getline(ifs, _line))
    {
        //解析每行的数据
        stringstream ss(_line);
        string _sub;

        //按照逗号分隔
        int col = 0;
        while (getline(ss, _sub, ',')){
            col++;
            switch (col) {
                case 1:
                    data_x.push_back(stof(_sub));
                    break;
                case 2:
                    data_y.push_back(stof(_sub));
                    break;
                case 3:
                    data_z.push_back(stof(_sub));
                    break;
                default:
                    break;
            }
        }

    }
    ifs.close();
    return true;
}

float calib_mag(vector<float> &data_x, vector<float> &data_y, vector<float> &data_z){
// 初始解
    float x_min = *min_element(data_x.begin(), data_x.end());
    float x_max = *max_element(data_x.begin(), data_x.end());
    float y_min = *min_element(data_y.begin(), data_y.end());
    float y_max = *max_element(data_y.begin(), data_y.end());
    float z_min = *min_element(data_z.begin(), data_z.end());
    float z_max = *max_element(data_z.begin(), data_z.end());
    float xc = 0.5f * (x_min + x_max);
    float yc = 0.5f * (y_min + y_max);
    float zc = 0.5f * (z_min + z_max);
    float a = 0.5f * abs(x_max - x_min);
    float b = 0.5f * abs(y_max - y_min);
    float c = 0.5f * abs(z_max - z_min);
//    xc=0;yc=0;zc=0;a=1;b=1;c=1;
    // 初始误差
    int L = data_x.size();
    float err = 0;
    for (int i = 0; i < L; i++) {
        // 代价函数
        err += abs((data_x[i] - xc) * (data_x[i] - xc) / (a * a) +
                   (data_y[i] - yc) * (data_y[i] - yc) / (b * b) +
                   (data_z[i] - zc) * (data_z[i] - zc) / (c * c) - 1);
    }
    printf("xc = %f yc = %f zc = %f, a = %f b = %f c= %f,init InitErr = %f\n",
           xc, yc, zc, a, b, c, err);

    // 开始计算
    float xc_last = xc;
    float yc_last = yc;
    float zc_last = zc;
    float a_last = a;
    float b_last = b;
    float c_last = c;
    float err_last = err;
    float r[6], xc_new, yc_new, zc_new, a_new, b_new, c_new, err_new;

    for (int i = 0; i < 1000; i++) {
        // 产生随机扰动 -0.5~0.5
        for (float &i : r) {
            i = (rand() % 100) * 0.01f - 0.5f;
        }
        xc_new = xc_last + r[0];
        yc_new = yc_last + r[1];
        zc_new = zc_last + r[2];
        a_new = abs(a_last + r[3]);
        b_new = abs(b_last + r[4]);
        c_new = abs(c_last + r[5]);
        err_new = 0;
        for (int j = 0; j < L; j++) {
            err_new += abs((data_x[j] - xc_new) * (data_x[j] - xc_new) / (a_new * a_new) +
                           (data_y[j] - yc_new) * (data_y[j] - yc_new) / (b_new * b_new) +
                           (data_z[j] - zc_new) * (data_z[j] - zc_new) / (c_new * c_new) - 1);
        }
        if (err_new < err_last) {
            xc_last = xc_new;
            yc_last = yc_new;
            zc_last = zc_new;
            a_last = a_new;
            b_last = b_new;
            c_last = c_new;
            err_last = err_new;
        }
    }

    printf("xc = %f yc = %f zc = %f, a = %f b = %f c= %f,last InitErr = %f\n",
           xc_last, yc_last, zc_last, a_last, b_last, c_last, err_last);
    float avr = (a_last + b_last + c_last) / 3;
    printf("mx = (mx - %f)*%f\n",xc_last,a_last/avr);
    printf("my = (my - %f)*%f\n",yc_last,b_last/avr);
    printf("mz = (mz - %f)*%f\n",zc_last,c_last/avr);
    return err_last;
}

// 地磁校准
int main() {

    srand((int) time(nullptr));  // 产生随机种子  把0换成NULL也行
    vector<float> data_x, data_y, data_z;
    // 读取数据
    load_csv(data_x, data_y, data_z);
    //校准，返回最终误差值
    float err = calib_mag(data_x, data_y, data_z);
    cout << err << endl;

    return 0;
}