欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136
標題:
超詳細講解：羅盤和加速度計校正方法
[打印本頁]
作者:
XFZ
時間:
2017-11-7 10:08
標題:
超詳細講解：羅盤和加速度計校正方法
羅盤和加計的校準是日常開發(fā)中最基礎(chǔ)的工作，特邀Echo老師對羅盤和加速度計校準的工程方法進行總結(jié)，為小伙伴你們解惑，是有此文。
作者信息
Echo，
本名
鄒佳池
，從事嵌入式軟件開發(fā)。
超詳細講解：羅盤和加速度計校正方法
(附C源代碼)
1.為什么要校正
我們都知道，羅盤是測量周圍的磁場強度，若不存在外在磁場的干擾，只存在地磁的話，理論上羅盤旋轉(zhuǎn)測得的磁場是一個圓球。
可是現(xiàn)實空間中，除了地磁場外，還存在其他的磁場干擾，這里我索性將它分為兩大類。
第一類
：地球空間中的磁場，這類磁場有個特點，就是隨著羅盤坐標系的轉(zhuǎn)動，磁場方向不變，類似地磁場。
第二類
：羅盤坐標空間中的磁場，這類磁場源一般是固定在飛機上的，所以隨著羅盤坐標系的轉(zhuǎn)動，磁場方向也跟著轉(zhuǎn)動。但是對于羅盤坐標系來說，卻是一個恒定值。
對于第一類的磁場，目前我了解到的還沒有什么好的方式可以進行校正（如果哪位大神知道，還請告知）。而我后面要介紹的校正方法，即是濾除第二類磁場的干擾，校正的思想即是基于最小二乘法的橢球擬合算法。
注：這個只能校正磁場強度固定不變的磁場，而對于電機這種變化的磁場，我沒有測驗過，不知道電機產(chǎn)生磁場的強度大小跟電機轉(zhuǎn)速的關(guān)系怎樣，如果誰有研究過的，還請告知，謝謝。
2.橢球擬合校正理論推導(dǎo)
網(wǎng)絡(luò)上有許多關(guān)于橢球擬合校正的論文，我都沒有細看，因為那些公式都寫得晦澀難懂，沒有那個耐心，我這里盡量用最簡潔的語言介紹校正方法的理論基礎(chǔ)。
首先建立數(shù)值模型，設(shè)測量值為：
,校正后的值為：，
平移參數(shù)為：
縮放參數(shù)為：
他們之間的關(guān)系如下所示：
我們校正后的目標就是使得校正值近似分布在一個圓球上，而圓球的公式大家都知道：x2+y2+z2=R2，故我們將校正后的值帶入圓球公式，與理論的圓球半徑平方做差，構(gòu)建誤差u：
將校正值用測量值替換，變?yōu)椋?br />
可以看到，這分明就是個橢圓公式嘛~
記：
則我們的誤差u可以寫為如下形式：
下面就是校正的核心思想了：假設(shè)我們有許多組數(shù)據(jù)，我們要求得一組參數(shù)，使得所有數(shù)據(jù)的誤差和最小，即∑u最小，但是由于u有正有負，所以符號相反的誤差有可能相互抵消。那么加絕對值呢？這個也不可取，因為對絕對值函數(shù)求極小值十分復(fù)雜。那么我們自然就想到對u求平方和，即：
U=∑u^2
我們把u看成一個未知數(shù)，這個函數(shù)是一個二次函數(shù)，其有極小值點。而為了求得這個極小值點，我們對其做偏導(dǎo)即可：
記：
則我們可以將偏導(dǎo)寫成如下形式：
B是已知的，P是我們待求的參數(shù)矩陣，故可以通過求齊次線性方程組，來求得P的各個參數(shù)的解。
齊次線性方程組求解的過程我這里就不詳細解釋了，我算法中使用的方法是經(jīng)典的高斯消元法，有興趣的可以仔細看看。
當我們求得P的各個未知數(shù)a,b,c,d,e,f,g后，需要通過這幾個參數(shù)反求出我們的偏移量(ox,oy,oz)和縮放量(gx,gy,gz)。
在反求解之前，我們先回到上一個式子，BxP=0。其實滿足這個式子的解P有無數(shù)組，我們可以將式子改寫成BxCP=0，C是一個任意常數(shù)，即
我們通過解線性方程組求得的只是這個解系中的一個基本解，所以我們首先要求出這個基本解的C。
其算式經(jīng)推導(dǎo)如下,帶入a,b,c,d,e,f,g即可求解：
C=(d2/a + e2/b + f2/c - 4g)/4R2 (R為理論圓球半徑)
ox=d/2a
oy=e/2b
oz=f/2c
gx=sqrt(a/C)
gy=sqrt(b/C)
gz=sqrt(c/C)
最后，將這六個參數(shù)回調(diào)到之前的式子中去，即完成校正。
3.校正程序源碼(C語言)
代碼太長了 就不貼出來了，放在網(wǎng)盤種大家下載
羅盤與加計校準方法 C源代碼
單片機源程序如下:
#include "stdafx.h"
#include "string.h"
#include "math.h"
#define MATRIX_SIZE 7
#define u8 unsigned char
double m_matrix[MATRIX_SIZE][MATRIX_SIZE+1];
int m = MATRIX_SIZE;
int n = MATRIX_SIZE+1;
double m_result[MATRIX_SIZE];
void DispMatrix(void);
double Abs(double a)
{
return a<0 ? -a : a;
}
u8 Equal(double a,double b)
{
return Abs(a-b) < 1e-6;
}
void ResetMatrix(void)
{
int row , column;
for(row = 0 ; row<m ; row++){
for(column = 0 ; column<n ; column++)
m_matrix[row][column] = 0.0f;
}
}
void CalcData_Input(double x , double y , double z)
{
double V[MATRIX_SIZE];
int row , column;
V[0] = x*x;
V[1] = y*y;
V[2] = z*z;
V[3] = x;
V[4] = y;
V[5] = z;
V[6] = 1.0;
//構(gòu)建VxVt矩陣(Vt為V的轉(zhuǎn)置)，并進行累加
for(row = 0 ; row<MATRIX_SIZE ; row++){
for(column = 0 ; column<MATRIX_SIZE ; column++){
m_matrix[row][column] += V[row]*V[column];
}
}
}
void SwapRow(int row1 , int row2)
{
int column;
double tmp;
for(column = 0 ; column<n ; column++){
tmp = m_matrix[row1][column];
m_matrix[row1][column] = m_matrix[row2][column];
m_matrix[row2][column] = tmp;
}
}
void MoveBiggestElement2Top(int s_row , int s_column)
{
int row,column;
for(row = s_row+1 ; row<m ; row++){
if( Abs(m_matrix[s_row][s_column])<Abs(m_matrix[row][s_column])){
SwapRow(s_row , row);
}
}
}
//高斯消元法，求行階梯型矩陣
u8 Matrix_GaussElimination(void)
{
int row,column,i,j;
double tmp;
for(row = 0,column=0 ; row<m-1 && column<n-1 ; row++,column++){
//將當前列最大的一行移上來
MoveBiggestElement2Top(row , column);
//整列都為0
if(Equal(m_matrix[row][column],0.0f)){
printf("qiyi matrix:%d %d\r\n" , row , column);
//DispMatrix();
//return 0;
row--;
continue;
}
//高斯消元
for(i = row+1 ; i<m ; i++){
if(Equal(m_matrix[i][column],0.0f))
continue; //為0，無需處理
tmp = m_matrix[i][column]/m_matrix[row][column];
for(j = column ; j<n ; j++){
m_matrix[i][j] -= m_matrix[row][j]*tmp;
}
}
DispMatrix();
printf("\r\n");
}
return 1;
}
//求行最簡型矩陣
int Matrix_RowSimplify(void)
{
int c = n;//返回值，表示(解的任意常量數(shù)+1)；
//
int row,column,k,s,t;
double tmp;
//
for(row=0,column=0;row<m && column<n;row++,column++)
{
if(Equal(m_matrix[row][column],0))//平移，找出本行第一個非零；
{
row--;
continue;
}
//
c--;//少一個常量；
//
//化a[i][j]為1；
tmp = 1 / m_matrix[row][column];
for(k=column;k<n;k++)//前面的"0"就不處理了；
m_matrix[row][k] *= tmp;
//
//化a[s][j]為0
for(s=0;s<row;s++)//下面的0也不用處理；
{
if(Equal(m_matrix[s][column],0))
continue;//已經(jīng)為0；
//
tmp = m_matrix[s][column] / m_matrix[row][column];
for(t=column;t<n;t++)
m_matrix[s][t] -= m_matrix[row][t]*tmp;
//
}
}
//
return c;
}
void Matrix_Solve(double* C , double* sol)
{
int row,column,i;
int any_sol[MATRIX_SIZE];
//找出任意解的位置
memset(any_sol , 0 , MATRIX_SIZE);
for(row=0,column=0 ; row<m && column<n-1 ; row++,column++){
if(Equal(m_matrix[row][column] , 0.0f)){
any_sol[column] = 1; //記錄任意解的位置
row--; //右移1列
}
}
//求解
row = 0;
for(column = 0 ; column<n-1 ; column++){
if(any_sol[column] == 1){ //任意解
sol[column] = C[column];
}else{
sol[column] = m_matrix[row][n-1];
//加上任意解
for(i = column+1 ; i<n-1 ; i++){
if(any_sol[i]==1 && !Equal(m_matrix[row][i],0.0f)){
sol[column] -= m_matrix[row][i]*C[i];
}
}
row++;
}
}
}
void DispMatrix(void)
{
int row,column;
for(row = 0 ; row<m ; row++){
for(column = 0 ; column<n ; column++){
printf("%.3f " , m_matrix[row][column]);
}
printf("\r\n");
}
}
void Calc_Process(double radius)
{
double C[MATRIX_SIZE];
double Res[MATRIX_SIZE];
int i;
double k;
ResetMatrix();
//輸入任意個數(shù)磁場測量點坐標，請盡量保證在橢球上分布均勻
CalcData_Input(7 , -7 , -2);
CalcData_Input(-1 , -7 , -2);
CalcData_Input(3 , 3 , -2);
CalcData_Input(3 , -17 , -2);
CalcData_Input(3 , -7 , 4);
CalcData_Input(3 , -7 , -8);
Matrix_GaussElimination();
Matrix_RowSimplify();
//賦值任意解參數(shù)值
……………………
…………限于本文篇幅 余下代碼請從51黑下載附件…………
復(fù)制代碼
所有資料51hei提供下載:
Calibration.rar
(2 KB, 下載次數(shù): 143)
2017-11-9 04:11 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
作者:
admin
時間:
2017-11-9 04:12
好資料，51黑有你更精彩!!!
作者:
xinzaizai
時間:
2017-11-24 16:50
好資料，幫了大忙
作者:
my51hei-huashen
時間:
2018-2-12 11:16
非常感謝！
作者:
iyzyh
時間:
2018-2-22 15:22
謝謝分享！剛剛好需要！
作者:
essien
時間:
2018-3-10 09:35
學(xué)習(xí)一下
作者:
essien
時間:
2018-3-10 09:36
謝謝分享！剛剛好需要，學(xué)習(xí)一下
作者:
KELIXIER
時間:
2018-4-30 20:27
好東西
作者:
KELIXIER
時間:
2018-4-30 20:28
謝謝分享！剛剛好需要，學(xué)習(xí)一下
作者:
摩天輪1111
時間:
2018-5-23 16:41
有點騙積分的感覺啊，我這里全貼出來
// 本程序?qū)θS散點數(shù)據(jù)進行橢球擬合
// 編 寫 人：鄒佳池
// 編寫日期：2016-5-25
// 技術(shù)交流QQ:529380360
// 請尊重作者的勞動成果，轉(zhuǎn)載請注明出處，謝謝
#include "stdio.h"
#include "string.h"
#include "math.h"
#define MATRIX_SIZE 7
#define u8 unsigned char
double m_matrix[MATRIX_SIZE][MATRIX_SIZE+1];
int m = MATRIX_SIZE;
int n = MATRIX_SIZE+1;
double m_result[MATRIX_SIZE];
void DispMatrix(void);
double Abs(double a)
{
return a<0 ? -a : a;
}
u8 Equal(double a,double b)
{
return Abs(a-b) < 1e-6;
}
void ResetMatrix(void)
{
int row , column;
for(row = 0 ; row<m ; row++){
for(column = 0 ; column<n ; column++)
m_matrix[row][column] = 0.0f;
}
}
void CalcData_Input(double x , double y , double z)
{
double V[MATRIX_SIZE];
int row , column;
V[0] = x*x;
V[1] = y*y;
V[2] = z*z;
V[3] = x;
V[4] = y;
V[5] = z;
V[6] = 1.0;
//構(gòu)建VxVt矩陣(Vt為V的轉(zhuǎn)置)，并進行累加
for(row = 0 ; row<MATRIX_SIZE ; row++){
for(column = 0 ; column<MATRIX_SIZE ; column++){
m_matrix[row][column] += V[row]*V[column];
}
}
}
void SwapRow(int row1 , int row2)
{
int column;
double tmp;
for(column = 0 ; column<n ; column++){
tmp = m_matrix[row1][column];
m_matrix[row1][column] = m_matrix[row2][column];
m_matrix[row2][column] = tmp;
}
}
void MoveBiggestElement2Top(int s_row , int s_column)
{
int row,column;
for(row = s_row+1 ; row<m ; row++){
if( Abs(m_matrix[s_row][s_column])<Abs(m_matrix[row][s_column])){
SwapRow(s_row , row);
}
}
}
//高斯消元法，求行階梯型矩陣
u8 Matrix_GaussElimination(void)
{
int row,column,i,j;
double tmp;
for(row = 0,column=0 ; row<m-1 && column<n-1 ; row++,column++){
//將當前列最大的一行移上來
MoveBiggestElement2Top(row , column);
//整列都為0
if(Equal(m_matrix[row][column],0.0f)){
printf("qiyi matrix:%d %d\r\n" , row , column);
//DispMatrix();
//return 0;
row--;
continue;
}
//高斯消元
for(i = row+1 ; i<m ; i++){
if(Equal(m_matrix[i][column],0.0f))
continue; //為0，無需處理
tmp = m_matrix[i][column]/m_matrix[row][column];
for(j = column ; j<n ; j++){
m_matrix[i][j] -= m_matrix[row][j]*tmp;
}
}
DispMatrix();
printf("\r\n");
}
return 1;
}
//求行最簡型矩陣
int Matrix_RowSimplify(void)
{
int c = n;//返回值，表示(解的任意常量數(shù)+1)；
//
int row,column,k,s,t;
float tmp;
//
for(row=0,column=0;row<m && column<n;row++,column++)
{
if(Equal(m_matrix[row][column],0))//平移，找出本行第一個非零；
{
row--;
continue;
}
//
c--;//少一個常量；
//
//這里不化成對角矩陣為1的矩陣，為了防止輸入的數(shù)據(jù)較大的時候，求出的解為接近于0值的情況
//tmp = 1 / m_matrix[row][column];
//for(k=column;k<n;k++)//前面的"0"就不處理了；
//m_matrix[row][k] *= tmp;
//
//化上三角矩陣為對角矩陣[/i][/i][/i]
[i][i][i] if(row == m-1)
m_matrix[row][column] = 0.0f; //強制為0，釋放一個自由度，否則很難有解
for(s=0;s<row;s++)//下面的0也不用處理；
{
if(Equal(m_matrix[s][column],0))
continue;//已經(jīng)為0；
//
tmp = m_matrix[s][column] / m_matrix[row][column];
for(t=column;t<n;t++)
m_matrix[s][t] -= m_matrix[row][t]*tmp;
//
}
}
DispMatrix();
printf("\r\n");
//
return c;
}
void Matrix_Solve(double* C , double* sol)
{
int row,column,i;
int any_sol[MATRIX_SIZE];
//找出任意解的位置
memset(any_sol , 0 , MATRIX_SIZE);
for(row=0,column=0 ; row<m && column<n-1 ; row++,column++){
if(Equal(m_matrix[row][column] , 0.0f)){
any_sol[column] = 1; //記錄任意解的位置
row--; //右移1列
}
}
//求解
row = 0;
for(column = 0 ; column<n-1 ; column++){
if(any_sol[column] == 1){ //任意解
sol[column] = C[column];
}else{
sol[column] = m_matrix[row][n-1];
//加上任意解
for(i = column+1 ; i<n-1 ; i++){
if(any_sol[i]==1 && !Equal(m_matrix[row][i],0.0f)){
sol[column] -= m_matrix[row][i]*C[i];
}
}
sol[column] /= m_matrix[row][column]; //除以對角線元素
row++;
}
}
}
void DispMatrix(void)
{
int row,column;
for(row = 0 ; row<m ; row++){
for(column = 0 ; column<n ; column++){
printf("%.3f " , m_matrix[row][column]);
}
printf("\r\n");
}
}
void Calc_Process(double radius)
{
double C[MATRIX_SIZE];
double Res[MATRIX_SIZE];
int i;
double k;
ResetMatrix();
//這里輸入任意個點的羅盤(加速度)參數(shù)，盡量在球面上均勻分布(可使用APM的六面采集法采集)
CalcData_Input(7 , -7 , -2);
CalcData_Input(-1 , -7 , -2);
CalcData_Input(3 , 3 , -2);
CalcData_Input(3 , -17 , -2);
CalcData_Input(3 , -7 , 4);
CalcData_Input(3 , -7 , -8);
Matrix_GaussElimination();
Matrix_RowSimplify();
for(i = 0 ; i<MATRIX_SIZE ; i++){
C[i] = 1000.0f;
}
Matrix_Solve(C , Res);
printf("a:%.2f b:%.2f c:%.2f d:%.2f e:%.2f f:%.2f g:%.2f\r\n" , Res[0],Res[1],Res[2],Res[3],Res[4],Res[5],Res[6]);
k = (Res[3]*Res[3]/Res[0]+Res[4]*Res[4]/Res[1]+Res[5]*Res[5]/Res[2] - 4*Res[6])/(4*radius*radius);
m_result[0] = sqrt(Res[0] / k);
m_result[1] = sqrt(Res[1] / k);
m_result[2] = sqrt(Res[2] / k);
m_result[3] = Res[3] / (2 * Res[0]);
m_result[4] = Res[4] / (2 * Res[1]);
m_result[5] = Res[5] / (2 * Res[2]);
printf("Xo:%f Yo:%f Zo:%f Xg:%f Yg:%f Zg:%f C:%f\r\n" , m_result[3],m_result[4],m_result[5],m_result[0],m_result[1],m_result[2],k);
while(1);
}
int main(int argc, char* argv[])
{
Calc_Process(2.0);
return 0;
}
復(fù)制代碼
作者:
nero0335
時間:
2019-3-6 10:02
樓主您好，附件里的代碼“Calibration.rar ”不是文章里講的代碼，“羅盤與加計校準方法”這個從哪里下載呢？
作者:
nero0335
時間:
2019-3-6 10:15
摩天輪1111 發(fā)表于 2018-5-23 16:41
有點騙積分的感覺啊，我這里全貼出來
兄弟，謝謝！！！！！
作者:
sf116
時間:
2019-3-29 11:52
謝謝分享！剛剛好需要！
作者:
yp9770
時間:
2019-5-24 08:31
謝謝分享！剛剛好需要！
作者:
liuzbl
時間:
2019-7-8 08:34
圖片怎么打不開呀？
作者:
peakzuo
時間:
2020-5-26 10:44
圖片看不到啊。
作者:
skychilang
時間:
2021-3-17 22:41
進來求知!雖看不太懂!但還是學(xué)習(xí)了
作者:
sczhangxx
時間:
2021-5-4 17:40
//輸入任意個數(shù)磁場測量點坐標，請盡量保證在橢球上分布均勻
CalcData_Input(7 , -7 , -2);
CalcData_Input(-1 , -7 , -2);
CalcData_Input(3 , 3 , -2);
CalcData_Input(3 , -17 , -2);
CalcData_Input(3 , -7 , 4);
CalcData_Input(3 , -7 , -8);
請教下，這是輸入的坐標還是磁力計采集數(shù)據(jù)呀，如果是坐標，磁力數(shù)據(jù)哪里輸入的呢
作者:
suzhao2008
時間:
2023-2-23 19:37
進來求知!雖看不太懂!但還是學(xué)習(xí)了
作者:
chenyucy72
時間:
2023-3-13 10:25
太強大了，雖然現(xiàn)在看不明白，感謝老師
作者:
兀自清涼
時間:
2023-9-12 15:05
圖片看不到了，有啥辦法可以看嗎，壓縮包里面也沒有講解的內(nèi)容，只有代碼了
歡迎光臨 (http://www.raoushi.com/bbs/)
Powered by Discuz! X3.1
Calibration.rar
(2 KB, 下載次數(shù): 143)
Calibration.rar
(2 KB, 下載次數(shù): 143)