一阶拟合算法C（六轴）

imu温飘曲线拟合算法经常用到一阶拟合，原始数据包含温度和加速度陀螺仪xyz轴数据，imu原始数据属于大量数据，需要采集大量数据样本（轴数据和温度），用到的基本函数：

求平均：

double get_mean(vector<double>::const_iterator it1, vector<double>::const_iterator it2)
{
    double sum = 0.0;
    double size = (double)(it2 - it1);

    while(it1 != it2)
    {
        sum += *it1;
        it1 ++;
    }

    return sum / size;
}

求方差：

double get_variance(vector<double>::const_iterator it1, vector<double>::const_iterator it2)
{
    double sum  =  0.0;
	double mean =  0.0;
	double var  =  0.0;

    if( it1 == it2 )
    {
        return 0;
    }
	
	vector<double>::size_type sz = it2 - it1;
	vector<double>::const_iterator it11 = it1;
	vector<double>::const_iterator it12 = it1;
	
	while(it11 != it2)
	{
		sum += *it11;
		it11 ++;
	}
	mean = sum / sz;
	
	while(it12 != it2)
	{
		var += (*it12 - mean) * (*it12 - mean);
		it12 ++;
	}
	
	return var / sz;
}

原始数据采集：

bool CImu::get_accdata(double temp_, double ax_, double ay_, double az_)
{
    acc_total_cnt++;
    get_acc_cnt++;

    // put data in vector
    acc_temp.push_back(temp_);
    ax_temp.push_back(ax_);
    ay_temp.push_back(ay_);
    az_temp.push_back(az_);
    

    // get mean data
    if(get_acc_cnt >= smooth_cnt)
    {
        atemp.push_back(get_mean(acc_temp.begin(),acc_temp.end()));
        ax.push_back(get_mean(ax_temp.begin(),ax_temp.end()));
        ay.push_back(get_mean(ay_temp.begin(),ay_temp.end()));
        az.push_back(get_mean(az_temp.begin(),az_temp.end()));

	accx_mean += get_mean(ax_temp.begin(),ax_temp.end());
	accy_mean += get_mean(ay_temp.begin(),ay_temp.end());
	accz_mean += get_mean(az_temp.begin(),az_temp.end());
	acc_mean_cnt ++;

	//*accdata << atemp.back() <<" "<<  ax.back() <<" "<<  ay.back() <<" "<< az.back() << endl;
	acc_temp.clear();
	ax_temp.clear();
	ay_temp.clear();
	az_temp.clear();
        get_acc_cnt = 0;
    }

    // cal cof
    if(temp_ >= stop_temperature || acc_total_cnt>=stop_time)
    {
    	cout << "atemp:" << temp_ << endl;
        return get_cof(true);
    }
    else
    {
        return false;
    }
}

bool CImu::get_gyrodata(double temp_, double gx_, double gy_, double gz_)
{
    gyro_total_cnt++;
    get_gyro_cnt++;

    // put data in vector
    gyro_temp.push_back(temp_);
    gx_temp.push_back(gx_);
    gy_temp.push_back(gy_);
    gz_temp.push_back(gz_);

    // get mean data
    if(get_gyro_cnt >= smooth_cnt)
    {
        gtemp.push_back(get_mean(gyro_temp.begin(),gyro_temp.end()));
        gx.push_back(get_mean(gx_temp.begin(),gx_temp.end()));
        gy.push_back(get_mean(gy_temp.begin(),gy_temp.end()));
        gz.push_back(get_mean(gz_temp.begin(),gz_temp.end()));

	//*gyrodata << gtemp.back() <<" "<<  gx.back() <<" "<<  gy.back() <<" "<< gz.back() << endl;
	
	gyro_temp.clear();
	gx_temp.clear();
	gy_temp.clear();
	gz_temp.clear();
        get_gyro_cnt = 0;
    }

    // cal cof
    if(temp_ >= stop_temperature || gyro_total_cnt>=stop_time)
    {
    	cout << "gtemp:" << temp_ << endl;
        return get_cof(false);
    }
    else
    {
        return false;
    }
}

采集完毕（达到时间点或者条件）：

bool CImu::get_cof(bool acc)
{
   if(acc){
    	a_cof.push_back(get_LS_cof(atemp,ax));
    	a_cof.push_back(get_LS_cof(atemp,ay));
    	a_cof.push_back(get_LS_cof(atemp,az));

	accx_mean = accx_mean / acc_mean_cnt;
	accy_mean = accy_mean / acc_mean_cnt;
	accz_mean = accz_mean / acc_mean_cnt;
	a_cof[0][1] -= accx_mean;
	a_cof[1][1] -= accy_mean;
	a_cof[2][1] -= accz_mean;
	
    	acc_ready = true;
   }else{
    	g_cof.push_back(get_LS_cof(gtemp,gx));
    	g_cof.push_back(get_LS_cof(gtemp,gy));
    	g_cof.push_back(get_LS_cof(gtemp,gz));
    	gyro_ready = true;
   }	

    return true;
}

拟合算法得出结果：

vector<double> get_LS_cof(const vector<double> &X, const vector<double> &Y)
{
    int data_size = X.size();
    // get mean
    double mean_X = get_mean(X.begin(),X.end());
    double mean_Y = get_mean(Y.begin(),Y.end());

    // get Lxx
    double Lxx = get_variance(X.begin(),X.end()) * (double)data_size;

    // get Lxy
    double Lxy = 0;
    for(int i=0;i<data_size;i++)
    {
        Lxy += (X[i] - mean_X) * (Y[i] - mean_Y);
    }

    // y = ax + b
    double a = Lxy / Lxx;
    double b = mean_Y - a * mean_X;

    vector<double> cof;
    cof.push_back(a);
    cof.push_back(b);

    return cof;
}

imu温飘拟合对象：

class CImu
{
    private:
        double stop_temperature;   //50
        int stop_time;             //10min 10*200*60
        int smooth_cnt;            //200
        int total_cnt;
	int acc_total_cnt,gyro_total_cnt;
        int get_data_cnt;
	int get_acc_cnt;
	int get_gyro_cnt;
	double accx_mean,accy_mean,accz_mean;
	unsigned int acc_mean_cnt;
	
        vector<double> atemp, gtemp, temp, ax, ay, az, gx, gy, gz;
        vector<double> acc_temp, gyro_temp, temp_temp, ax_temp, ay_temp, az_temp, gx_temp, gy_temp, gz_temp;
        vector< vector<double> > a_cof,g_cof;
        bool get_cof(bool acc);
    public:
	bool acc_ready,gyro_ready;
	//ofstream* accdata;
	//ofstream* gyrodata;
        CImu(double stop_temperature_ = 50, int smooth_cnt_ = 200, int stop_time_ = 120000) : 
        stop_temperature(stop_temperature_), smooth_cnt(smooth_cnt_), stop_time(stop_time_) {
	total_cnt=0; get_data_cnt=0;acc_total_cnt=0;gyro_total_cnt=0;get_acc_cnt = 0;get_gyro_cnt = 0;acc_ready = gyro_ready = false;
	//accdata = new ofstream("/mnt/hgfs/Vmshare/hiddevice/HID_COMMUNICATION/acc.txt");
	//gyrodata = new ofstream("/mnt/hgfs/Vmshare/hiddevice/HID_COMMUNICATION/gyro.txt");
	accx_mean = accy_mean = accz_mean = 0;
	acc_mean_cnt = 0;
	}
        ~CImu(){
	
	}
        bool get_data(double temp_, double ax_, double ay_, double az_, double gx_, double gy_, double gz_);
	bool get_accdata(double temp_, double ax_, double ay_, double az_);
	bool get_gyrodata(double temp_, double gx_, double gy_, double gz_);
	vector< vector<double> > get_acof_data() { return a_cof;}
	vector< vector<double> > get_gcof_data() { return g_cof;}
};