[资料]

六轴姿态解算EKF

2018-9-11 23:34:29 1397

0 #include "arhs_ekf.h" #include "stdlib.h" #include float Pitch,Roll,Yaw; float Ak[16]={0};//Ô¤²â¾ØÕó float q[4]={1,0,0,0};//³õÊ¼»¯ËÄÔªÊý float Pk1[16]= {0.0001 ,0.00001 ,0.00001 ,0.00001, 0.00001,0.0001 ,0.00001 ,0.00001, 0.00001,0.00001 ,0.0001 ,0.00001, 0.00001,0.00001 ,0.00001 ,0.0001 };//0.001Ð·½²î¾ØÕó³õÊ¼Öµ float Qk[16] ={0.000001 , 0 , 0 , 0 , 0 , 0.000001 , 0 , 0 , 0 , 0 , 0.000001 , 0 , 0 , 0 , 0 , 0.000001 };//Ô¤²â¸ßË¹ÔëÉù float Rk1[9]= {300,0,0, 0,300,0, 0,0,300};//²âÁ¿¸ßË¹ÔëÉù float Eye[16]={1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};//4Î¬µ¥Î»¾ØÕó float Pk[16]={0};//Ô¤²âÐ·½²î¾ØÕó float R2[3]={0};//×ø±ê×ª»»¾ØÕóµÚ¶þÁÐ float R3[3]={0};//×ø±ê×ª»»¾ØÕóµÚÈýÁÐ float J3[12]={0};//R3µÄÑÅ¿É±È¾ØÕó float Kk1[12]={0};//¿¨¶ûÂüÔöÒæKk1 float Pk_1[16]={0};//¼ÓËÙ¶ÈÔ¤²âÐ·½²î void MatrixAdd(float a, float b, float c, unsigned char m, unsigned char n); void MatrixMinus(float a, float b, float c, unsigned char m, unsigned char n); void MatrixTrans(float a, float c, unsigned char m, unsigned char n); void MatrixMul(float a, float b, float c, unsigned char m, unsigned char p, unsigned char n); void MatrixCal(float a, float b, float c, unsigned char n); unsigned char Gauss_Jordan(float a, unsigned char n); float invs_qrt(float x); //¾ØÕóÔËËãº¯Êý³õÊ¼»¯ float g=9.79973; float TEMP3[12]={0};//ÖÐ¼ä±äÁ¿TEMP3 Hk1×ªÖÃ float TEMP4[12]={0};//ÖÐ¼ä±äÁ¿TEMP4 float TEMP5[9]={0};//ÖÐ¼ä±äÁ¿TEMP5 float TEMP6[12]={0};//ÖÐ¼ä±äÁ¿TEMP6 float TEMP7[9]={0};//ÖÐ¼ä±äÁ¿TEMP7 float TEMP10[16]={0}; float norm_a=0;//·ÀÖ¹Êý¾Ý·¢É¢ float norm_q=0; float norm_qt=0; float norm_g=0; float TEMP8[16]={0};//ÖÐ¼ä±äÁ¿ float TEMP9[16]={0}; float Hk1[12]={0};//AccÑÅ¿É±È¸³Öµ float qe1[4]={0};//¼ÓËÙ¶È¼ÆËã³öµÄËÄÔªÊýÎó²î float ae1[3]={0};//ÖÐ¼ä±äÁ¿ void ARHS_EkfFilter(float ax,float ay,float az,float mx,float my,float mz, float dT) { float gx=Gyro_Filter_Data.XGyro_Rates;//½ÇËÙ¶È»¡¶È»¯ float gy=Gyro_Filter_Data.YGyro_Rates; float gz=Gyro_Filter_Data.ZGyro_Rates; q[0]+= (-gxq[1]-gyq[2]-gzq[3])dT0.5;//halfTÎªÖÜÆÚ q[1]+= ( gxq[0]+gzq[2]-gyq[3])dT0.5; q[2]+= ( gyq[0]-gzq[1]+gxq[3])dT0.5; q[3]+= ( gzq[0]+gyq[1]-gxq[2])dT0.5; norm_q=invs_qrt(q[0]q[0]+q[1]q[1]+q[2]q[2]+q[3]q[3]); //Ô¤²âËÄÔªÊý¹éÒ»»¯ q[0]=q[0]norm_q; q[1]=q[1]norm_q; q[2]=q[2]norm_q; q[3]=q[3]norm_q;//X=AXk-1 Ak[0]= 1.0; Ak[1]= -0.5gxdT; Ak[2]= -0.5gydT; Ak[3]= -0.5gydT; Ak[4]= 0.5gxdT; Ak[5]= 1.0; Ak[6]= 0.5gzdT; Ak[7]= -0.5gydT; Ak[8]= 0.5gydT; Ak[9]= -0.5gzdT; Ak[10]= 1.0; Ak[11]= 0.5gxdT; Ak[12]= 0.5gzdT; Ak[13]= 0.5gydT; Ak[14]= -0.5gxdT; Ak[15]= 1.0;//Ô¤²â¾ØÕó MatrixCal(Ak,Pk1,TEMP10,4); MatrixAdd(TEMP10,Qk,Pk,4,4);//Pk=AkPk_1Ak'+Qk ¹«Ê½2 if((axayaz)==0) return;//Èç¹ûÊý¾Ý²»¶Ô¾Í·µ»Ø norm_a=invs_qrt(axax+ayay+azaz); ax=axnorm_ag; ay=aynorm_ag; az=aznorm_ag;//¼ÓËÙ¶È¼ÆÊý¾Ý¹éÒ»»¯ R3[0]=(2.0q[1]q[3]-2.0q[0]q[2])g; R3[1]=(2.0q[2]q[3]+2.0q[0]q[1])g; R3[2]=(2.0q[0]q[0]+2.0q[3]q[3]-1)g; //////////////////////////////// J3[0]= -2.0q[2]g; J3[1]= 2.0q[3]g; J3[2]= -2.0q[0]g; J3[3]= 2.0q[1]g; J3[4]= 2.0q[1]g; J3[5]= 2.0q[0]g; J3[6]= 2.0q[3]g; J3[7]= 2.0q[2]g; J3[8]= 2.0q[0]g; J3[9]= -2.0q[1]g; J3[10]= -2.0q[2]g; J3[11]= 2.0q[3]g; ///////////////////// for(uint8_t i=0;i<12;i++ ) { Hk1=J3; } //////////// MatrixTrans(Hk1,TEMP3,3,4);//TEMP3Îª×ªÖÃ MatrixMul(Hk1,Pk,TEMP6,3,4,4);//34 MatrixMul(TEMP6,TEMP3,TEMP5,3,4,3);//33 tEMP5 MatrixAdd(TEMP5,Rk1,TEMP7,3,3);//TEMP7 33 Gauss_Jordan(TEMP7,3);//¾ØÕóÇóÄæ TEMP7 MatrixMul(Pk,TEMP3,TEMP4,4,4,3);//PkHk1' TEMP4 43 MatrixMul(TEMP4,TEMP7,Kk1,4,3,3);//43 Kk1=P_kHk1'inv(Hk1P_kHk1'+Rk1);¹«Ê½3 // ae1[0]=ax-R3[0]; ae1[1]=ay-R3[1]; ae1[2]=az-R3[2]; //ae1[0]=ayR3[2]-azR3[1]; //ae1[1]=azR3[0]-axR3[2]; //ae1[2]=axR3[1]-ayR3[0]; qe1[0]=Kk1[0]ae1[0]+Kk1[1]ae1[1]+Kk1[2]ae1[2]; qe1[1]=Kk1[3]ae1[0]+Kk1[4]ae1[1]+Kk1[5]ae1[2]; qe1[2]=Kk1[6]ae1[0]+Kk1[7]ae1[1]+Kk1[8]ae1[2]; qe1[3]=Kk1[9]ae1[0]+Kk1[10]ae1[1]+Kk1[11]ae1[2];//¼ÓËÙ¶È¹À¼ÆµÄÎó²îqe1 q[0]=q[0]+qe1[0]; q[1]=q[1]+qe1[1]; q[2]=q[2]+qe1[2]; q[3]=q[3]+qe1[3];//¹À¼Æ×îÓÅËÄÔªÊý&&µ¥Î»»¯ /////////////////////////////// MatrixMul(Kk1,Hk1,TEMP8,4,3,4);//44 MatrixMinus(Eye,TEMP8,TEMP9,4,4); MatrixMul(TEMP9,Pk,Pk_1,4,4,4); //Pk_1=(eye(4)-Kk1Hk1)P_k; for(uint8_t i=0;i<16;i++) { Pk1 = Pk_1; }//¸³Ö norm_qt=invs_qrt(q[0]q[0]+q[1]q[1]+q[2]q[2]+q[3]q[3]); q[0]=q[0]norm_qt; q[1]=q[1]norm_qt; q[2]=q[2]norm_qt; q[3]=q[3]norm_qt; Roll = atan2(2 q[2]q[3] + 2 q[0]q[1], -2 q[1]q[1]- 2 q[2]q[2] + 1)57.3; // roll Pitch = asin(-2 q[1]q[3] + 2 q[0]q[2])57.3; // pitch Yaw = atan2(2 q[1]q[2] + 2 q[0]q[3], -2 q[2]q[2] - 2 q[3]q[3] + 1) 57.3; // yaw } //¾ØÕóÇóÄæ unsigned char Gauss_Jordan(float a, unsigned char n) { signed char i,j,k,l,u,v; signed char is[3]; signed char js[3]; float d,p; for (k=0; k<=n-1; k++) { d=0.0; for (i=k; i<=n-1; i++) for (j=k; j<=n-1; j++) { l=in+j; p=fabs(a[l]); if (p>d) { d=p; is[k]=i; js[k]=j;} } if (d+1.0==1.0) { return(0); } if (is[k]!=k) for (j=0; j<=n-1; j++) { u=kn+j; v=is[k]n+j; p=a; a=a[v]; a[v]=p; } if (js[k]!=k) for (i=0; i<=n-1; i++) { u=in+k; v=in+js[k]; p=a; a=a[v]; a[v]=p; } l=kn+k; a[l]=1.0/a[l]; for (j=0; j<=n-1; j++) if (j!=k) { u=kn+j; a=aa[l];} for (i=0; i<=n-1; i++) if (i!=k) for (j=0; j<=n-1; j++) if (j!=k) { u=in+j; a=a-a[in+k]a[kn+j]; } for (i=0; i<=n-1; i++) if (i!=k) { u=in+k; a=-aa[l];} } for (k=n-1; k>=0; k--) { if (js[k]!=k) for (j=0; j<=n-1; j++) { u=kn+j; v=js[k]n+j; p=a; a=a[v]; a[v]=p; } if (is[k]!=k) for (i=0; i<=n-1; i++) { u=in+k; v=in+is[k]; p=a; a=a[v]; a[v]=p; } } return(1); } //¾ØÕóÏà¼Ó void MatrixAdd(float a, float b, float c, unsigned char m, unsigned char n) { unsigned char i; for (i=0; i { c = a + b; } } //¾ØÕóÏà¼õ void MatrixMinus(float a, float b, float c, unsigned char m, unsigned char n) { unsigned char i; for (i=0; i { c = a - b; } } //Çó×ªÖÃ void MatrixTrans(float a, float c, unsigned char m, unsigned char n) { unsigned char i,j; for (i=0; i { for (j=0; j { c[im+j] = a[jn+i]; //??mn???,im+j??i?j????; ??nm???,in+j??i?j???? } } } //¾ØÕóÏà³Ë void MatrixMul(float a, float b, float c, unsigned char m, unsigned char p, unsigned char n) { unsigned char i,j,k; //???? for (i=0; i { for (j=0; j { c[in+j] = 0.0; for (k=0; k { //??????????? c[in+j] += a[ip+k] b[kn+j]; //a[ip+k]: ??k???,???a[]????i? //b[kn+j]: ??k???,???a[]????j? } } } } //¼ÆËãABA' void MatrixCal(float a, float b, float c, unsigned char n) { float Temp1[16] = {0}; float Temp2[16] = {0}; MatrixMul(a, b, Temp1, n, n, n); MatrixTrans(a, Temp2, n, n); MatrixMul(Temp1, Temp2, c, n, n, n); } //¿ìËÙÆ½·½¸ùËã·¨ float invs_qrt(float x) { float halfx = 0.5f * x; float y = x; long i = (long)&y; i = 0x5f3759df - (i>>1); y = (float)&i; y = y * (1.5f - (halfx * y * y)); return y; } 0
举报淘帖0 只看该作者相关推荐 • 《DNK210使用指南 -CanMV版 V1.0》第二十三章六轴传感器——姿态解算实验 1448 • FPGA实现MPU6050姿态解算 770 • MPU6050姿态融合解算的相关资料推荐 934 • 使用MPU6050硬件DMP解算姿态 1173 • 基于STM32的四轴无人机的姿态解算与控制系统设计 2299 • 【案例分享】四轴飞行器6050六轴传感器软件姿态解算 2795 • 请问四轴姿态解算误差修正的原理是什么？ 1292 • 请问MPU6050的软件解算姿态和DMP解算姿态各自的优缺点是什么？ 4280 • 【CANNON申请】姿态解算 2191 • 四轴姿态解算 3186