|
|
本帖最后由 onev 于 2014-5-1 22:25 编辑
/**********************************************************************************************
说明：本程序是在VS2012环境下写就。虽然是在C++环境下写就但实际还是C编程，故构造函数、析构函数均略去。
鉴于C++的流输入和流输出的使用更为方便简洁，因而使用cin、cout代替printf（）、scanf（）等函数。
描述：
作者：onev
时间：2014
***********************************************************************************************/
#include <iostream>
#include <string>
#include <cmath>
//#include <cstdio>
using namespace std;
#define NUM 6 //线性方程组大小
//http://zh.wikipedia.org/wiki/%E7%BA%BF%E6%80%A7%E6%96%B9%E7%A8%8B%E7%BB%84
//double matrix_A[4][4] = {{5, -1, -1, -1},{-1, 10, -1, -1},{-1, -1, 5, -1},{-1, -1, -1, 10}};//矩阵A
//double matrix_b[4] = {-4, 12, 8, 34};//矩阵b
//double matrix_x[4] = {0, 0, 0, 0};//解
float matrix_Q[6][3] = {{-135, 54, -152},{92, -93, -164},{72, 86, -165},{66, 151, 66},{-87,153,-108},{-82,-112,-167}};//6组电子罗盘数据，顺序X、Y、Z
double matrix_A[NUM][NUM] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};//A矩阵由后面的PP函数产生
double matrix_b[NUM] = {0,0,0,0,0,0};//同上
double matrix_x[NUM] = {0, 0, 0, 0, 0, 0};
double matrix_M[7] = {0};
void PP(void)
{
double matrix_temp1[NUM][NUM] = {0};
for (int i = 0; i < NUM; i++)
{
matrix_temp1[0] = matrix_Q[0]*matrix_Q[0];
matrix_temp1[1] = matrix_Q[1]*matrix_Q[1];
matrix_temp1[2] = matrix_Q[2]*matrix_Q[2];
matrix_temp1[3] = matrix_Q[0];
matrix_temp1[4] = matrix_Q[1];
matrix_temp1[5] = matrix_Q[2];
}
for (int j = 0; j < NUM; j++)
{
for (int i = 0; i < NUM; i++)
{
matrix_A[j][0] += matrix_temp1[j]*matrix_temp1[0];
matrix_A[j][1] += matrix_temp1[j]*matrix_temp1[1];
matrix_A[j][2] += matrix_temp1[j]*matrix_temp1[2];
matrix_A[j][3] += matrix_temp1[j]*matrix_temp1[3];
matrix_A[j][4] += matrix_temp1[j]*matrix_temp1[4];
matrix_A[j][5] += matrix_temp1[j]*matrix_temp1[5];
}
}
for (int i = 0; i < NUM; i++)
{
for (int j = 0; j < NUM; j++)
{
matrix_b += -matrix_temp1[j];
}
cout << "matrix_b[" << i <<"]" << matrix_b << endl;
cout << "*********************************************" << endl;
}
for (int i = 0; i < NUM; i++)
{
for(int j = 0; j < NUM; j++)
{
cout << "matrix_A[" << i <<"]" << "["<< j << "]" << matrix_A[j] << endl;
}
}
}
/******************************************************************************
函数 ： SOR()
功能 ： SOR迭代法解线性方程组
调用 ： 外部
说明 ： 松弛因子0~1 比如0.9 精度自己决定 比如0.000001
返回值： 有
******************************************************************************/
int SOR()
{
int i = 0, j = 0,k = 0;
double w = 0, precision = 0, temp = 0, x[NUM] = {0, 0, 0, 0};//w:松弛因子 precision：精度
cout << "请输入松弛因子w： "<< endl;
cin >> w ;
cout << "请输入精度： "<< endl;
cin >> precision;
for(k = 0; k < 1000; k++)//迭代次数最多不超过100
{
for(i = 0; i < NUM; i++)
{
temp = 0;
for(j = 0; j < NUM; j++)
{
if(j != i)
temp += matrix_A[j]*matrix_x[j];
}
matrix_x = (1-w)*x+w*(matrix_b-temp)/matrix_A;
cout << matrix_x << " ";
if(i == 3)
cout << endl;
}
for(i = 0; i < NUM; i++)
{
if(fabs(matrix_x-x) < precision)
{
if(i == 3)
{
cout << endl;
cout << "您输入的松弛因子w = " << w << endl;
cout << " 输入的精度precision = " << precision << endl;
cout << "迭代次数： " << k << endl;
cout << "迭代结果："<< endl;
for (int i = 0; i < NUM; i++)
{
cout << "x" << i << ":" << matrix_x << endl;
}
/* cout <<"x1 = " << matrix_x[0] << " x2 = " << matrix_x[1];
cout << "x3 = " << matrix_x[2] << " x4 = " << matrix_x[3];
cout << "x5 = " << matrix_x[4] << " x6 = " << matrix_x[5] << endl;*/
return 0;
}
}
else
break;
}
for(i = 0; i < NUM; i++)
x = matrix_x;
}
}
void OO(void)
{
matrix_M[0] = matrix_x[3]/2/matrix_x[0];
matrix_M[1] = matrix_x[4]/2/matrix_x[1];
matrix_M[2] = matrix_x[5]/2/matrix_x[2];
matrix_M[3] = 1.0/(matrix_x[3]*matrix_x[3]/matrix_x[0]/4
+matrix_x[4]*matrix_x[4]/matrix_x[1]/4
+matrix_x[5]*matrix_x[5]/matrix_x[2]/4
-1);//这里把地球磁场大小设成了1
matrix_M[4] = sqrtl(/*abs*/(matrix_M[3])*matrix_x[0]);
matrix_M[5] = sqrtl(/*abs*/(matrix_M[3])*matrix_x[1]);
matrix_M[6] = sqrtl(/*abs*/(matrix_M[3])*matrix_x[2]);
for(int i = 0; i < 7; i++)
{
cout << endl;
cout << "******************" << endl;
cout << "matrix_M[" << i << "]:" << matrix_M << endl;
}
}
int main()
{
PP();
SOR();
OO();
cout << (matrix_x[3]*matrix_x[3]/matrix_x[0]/4
+matrix_x[4]*matrix_x[4]/matrix_x[1]/4
+matrix_x[5]*matrix_x[5]/matrix_x[2]/4) << endl;
}
说明：测量值[Xm,Ym,Zm] 校正后[Xc,Yc,Zc] 平移参数[Ox,Oy,Oz] 缩放参数[gx,gy,gz] 关系：Xc=(Xm+Ox)*gx
Yc=(Ym+Oy)*gy
Zc=(Zm+Oz)*gz
PP(); OO();函数不解释。
matrix_M[0]对应Ox；matrix_M[1]对应Oy；matrix_M[2]对应Oz；
matrix_M[4]对应gx； matrix_M[5]对应gy； matrix_M[6]对应gz；
这是我自己做的椭球拟合，最小二乘法。
问题来了：用AHRSupdate进行姿态解算时在开始的大概十几二十秒内会乱七八糟的飘，过了这会儿之后基本回归正常，但偏航角yaw会有偏移这时，这说的偏移不是漂移。过了刚刚说的时间段后姿态很稳定，yaw不飘，放那不动三个角都不会动，或者你动了之后放那也不会飘，关键在于为什么开始时会有那种乱七八糟的飘呢？一直找不到原因，另外因为还没装MATLAB，求教能否检验一下我的椭球拟合是否正确呢？
|
阿莫论坛20周年了！感谢大家的支持与爱护！！
阿莫论坛才是最爱国的，关心国家的经济、社会的发展、担心国家被别国牵连卷入战争、知道珍惜来之不易的和平发展，知道师夷之长，关注世界的先进文化与技术，也探讨中国文化的博大精深，也懂得警惕民粹主义的祸国殃民等等等等，无不是爱国忧民的表现。（坛友：tianxian）
|
[复制链接]
发表于 2014-5-1 19:01:26
楼主
