受這篇帖子啟發(fā)，http://www.zg4o1577.cn/bbs/dpj-97791-1.html
超詳細講解：羅盤和加速度計校正方法
但是實話說，這個代碼好像還是有點問題的，超過6個采樣點就不能用了，少于6個采樣點也不能用，
沒辦法，自己就重新寫了一遍，
我的思路是基于最小二乘法的橢球擬合，正統(tǒng)樸素方法
可以轉(zhuǎn)看知乎
https://www.zhihu.com/people/mo-tian-lun-1111/posts
也可以直接看這里貼的代碼，回復可見把，免的雁過不留毛的
//橢球校準.cpp
//最小二乘的橢球擬合
//((x-x0)/A)^2 + ((y-y0)/B)^2 + ((z-z0)/C)^2 = 1 的空間任意橢球方程式
//x^2 + a*y^2 + b*z^2 + c*x + d*y + e*z + f = 0 簡化后的方程
//問題轉(zhuǎn)換為由a,b,c,d,e,f,來求解x0，y0，z0 以及 A,B,C
//作者：摩天輪1111
//知乎ID：摩天輪1111 轉(zhuǎn)載請注明出處 尊重勞動者成果
#include "stdafx.h"
#include "stdio.h"
#include "string.h"
#include "math.h"
#define MATRIX_SIZE 6
#define u8 unsigned char
double m_matrix[MATRIX_SIZE][MATRIX_SIZE + 1];//系數(shù)矩陣
double solve[MATRIX_SIZE] = { 0 };//方程組的解對應最小二乘橢球擬合中的，a，b，c，d，e，f，
double m_result[MATRIX_SIZE];
int N = 0;//計算累計的采樣點次數(shù)的
//取絕對值
double Abs(double a)
{
return a < 0 ? -a : a;
}
//把矩陣系數(shù)全部清除為0
void ResetMatrix(void)
{
for (u8 row = 0; row < MATRIX_SIZE; row++)
{
for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
m_matrix[row][column] = 0.0f;
}
}
//把輸入的數(shù)據(jù)先生成矩陣的元素的總和
void CalcData_Input(double x, double y, double z)
{
double V[MATRIX_SIZE + 1];
N++;
V[0] = y*y;
V[1] = z*z;
V[2] = x;
V[3] = y;
V[4] = z;
V[5] = 1.0;
V[6] = -x*x;
//構建系數(shù)矩陣，并進行累加
for (u8 row = 0; row < MATRIX_SIZE; row++)
{
for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
{
m_matrix[row][column] += V[row] * V[column];
}
}
//b向量是m_matrix[row][6]
}
//化簡系數(shù)矩陣，把除以N帶上
void CalcData_Input_average()
{
for (u8 row = 0; row < MATRIX_SIZE; row++)
for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
m_matrix[row][column] /= N;
//b向量是m_matrix[row][6]
}
//顯示出來系數(shù)矩陣和增廣矩陣[A|b]
void DispMatrix(void)
{
for (u8 row = 0; row < MATRIX_SIZE; row++)
{
for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
{
printf("%23f ", m_matrix[row][column]);
if (column == MATRIX_SIZE - 1)
printf("|");
}
printf("\r\n");
}
printf("\r\n\r\n");
}
//交換兩行元素位置
void Row2_swop_Row1(int row1, int row2)
{
double tmp = 0;
for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
{
tmp = m_matrix[row1][column];
m_matrix[row1][column] = m_matrix[row2][column];
m_matrix[row2][column] = tmp;
}
}
//用把row行的元素乘以一個系數(shù)k
void k_muiltply_Row(double k, int row)
{
for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
m_matrix[row][column] *= k;
}
//用一個數(shù)乘以row1行加到row2行上去
void Row2_add_kRow1(double k, int row1, int row2)
{
for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
m_matrix[row2][column] += k*m_matrix[row1][column];
}
//列主元，第k次消元之前，把k行到MATRIX_SIZE的所有行進行冒泡排出最大，排序的依據(jù)是k列的元素的大小
void MoveBiggestElement_to_Top(int k)
{
int row = 0, column = 0;
for (row = k + 1; row < MATRIX_SIZE; row++)
{
if (Abs(m_matrix[k][k]) < Abs(m_matrix[row][k]))
{
Row2_swop_Row1(k, row);
}
}
}
//高斯消元法，求行階梯型矩陣
u8 Matrix_GaussElimination(void)
{
double k = 0;
for (u8 cnt = 0; cnt < MATRIX_SIZE; cnt++)//進行第k次的運算，主要是針對k行以下的行數(shù)把k列的元素都變成0
{
//把k行依據(jù)k列的元素大小，進行排序
MoveBiggestElement_to_Top(cnt);
if (m_matrix[cnt][cnt] == 0)
return(1);//返回值表示錯誤
//把k行下面的行元素全部消成0，整行變化
for (u8 row = cnt + 1; row < MATRIX_SIZE; row++)
{
k = -m_matrix[row][cnt] / m_matrix[cnt][cnt];
Row2_add_kRow1(k, cnt, row);
}
DispMatrix();
}
return 0;
}
//求行最簡型矩陣，即把對角線的元素全部化成1
void Matrix_RowSimplify(void)
{
double k = 0;
for (u8 row = 0; row < MATRIX_SIZE; row++)
{
k = 1 / m_matrix[row][row];
k_muiltply_Row(k, row);
}
DispMatrix();
}
//求非齊次線性方程組的解
void Matrix_Solve(double* solve)
{
for (short row = MATRIX_SIZE - 1; row >= 0; row--)
{
solve[row] = m_matrix[row][MATRIX_SIZE];
for (u8 column = MATRIX_SIZE - 1; column >= row + 1; column--)
solve[row] -= m_matrix[row][column] * solve[column];
}
printf(" a = %f| b = %f| c = %f| d = %f| e = %f| f = %f ", solve[0], solve[1], solve[2], solve[3], solve[4], solve[5]);
printf("\r\n");
printf("\r\n");
}
//整個橢球校準的過程
void Ellipsoid_fitting_Process(void)
{
ResetMatrix();
//這里輸入任意個點加速度參數(shù)，盡量在球面上均勻分布
CalcData_Input(87, -52, -4454);
CalcData_Input(301, -45, 3859);
CalcData_Input(274, 4088, -303);
CalcData_Input(312, -4109, -305);
CalcData_Input(-3805, -24, -390);
CalcData_Input(4389, 6, -228);
CalcData_Input(261, 2106, -3848);
CalcData_Input(327, -2047, -3880);
CalcData_Input(-1963, -13, -3797);
CalcData_Input(3024, 18, -3449);
CalcData_Input_average();//對輸入的數(shù)據(jù)到矩陣元素進行歸一化
DispMatrix();//顯示原始的增廣矩陣
if (Matrix_GaussElimination()) //求得行階梯形矩陣
printf("the marix could not be solved\r\n");
else
{
Matrix_RowSimplify();//化行最簡形態(tài)
Matrix_Solve(solve);//求解a,b,c,d,e,f
double a = 0, b = 0, c = 0, d = 0, e = 0, f = 0;
a = solve[0];
b = solve[1];
c = solve[2];
d = solve[3];
e = solve[4];
f = solve[5];
double X0 = 0, Y0 = 0, Z0 = 0, A = 0, B = 0, C = 0;
X0 = -c / 2;
Y0 = -d / (2 * a);
Z0 = -e / (2 * b);
A = sqrt(X0*X0 + a*Y0*Y0 + b*Z0*Z0 - f);
B = A / sqrt(a);
C = A / sqrt(b);
printf(" ((x - x0) / A) ^ 2 + ((y - y0) / B) ^ 2 + ((z - z0) / C) ^ 2 = 1 Ellipsoid result as below：\r\n");
printf("\r\n");
printf(" X0 = %f| Y0 = %f| Z0 = %f| A = %f| B = %f| C = %f \r\n", X0, Y0, Z0, A, B, C);
}
while (1);
}
int main(int argc, char* argv[])
{
Ellipsoid_fitting_Process();
return 0;
}
|
[復制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
