【求助】关于四元数与EKF融合的问题，附代码。
float EKF_Wx, EKF_Wy,EKF_Wz;float Roll,Pitch,Yaw;
float A,X,X_tmp,P,P_tmp,K,Z,Z_tmp,H,R,Q,A,I;
/*******************************************************************************
* Function Name: A_Maxup
* Description : A矩阵建立及更新
* Input : 系统嘀嗒Cycle_t，参数K，参数B
* Output : A矩阵
* Return : null
*******************************************************************************/
void A_Maxup(float Cycle_t,float delta_K,float delta_B)
{
float X_tmp,Y_tmp,Z_tmp;
X_tmp=delta_B*EKF_Wx*Cycle_t;
Y_tmp=delta_B*EKF_Wy*Cycle_t;
Z_tmp=delta_B*EKF_Wz*Cycle_t;
A = delta_K;
A = -X_tmp ;
A = -Y_tmp ;
A = -Z_tmp ;
A =X_tmp;
A = delta_K;
A =Z_tmp ;
A = -Y_tmp ;
A =Y_tmp ;
A = -Z_tmp ;
A = delta_K;
A =X_tmp ;
A = Z_tmp ;
A =Y_tmp ;
A = -X_tmp ;
A = delta_K;
}
/*******************************************************************************
* Function Name: H_Maxup
* Description : H矩阵建立及更新
* Input : X矩阵，H矩阵
* Output : H矩阵
* Return : null
*******************************************************************************/
void H_Maxup(float X,float H)
{
float dcm,H_tmp;
float X12,X22,X32,X_01,X_02,X_03,X_12,X_13,X_23;
//关于H矩阵，因为变换为非线性，故采用EKF滤波，H阵为dh/dq；
//先由X矩阵归一化；
Q_norm(X);
//由归一化的X矩阵得到方向余弦矩阵dcm
X12 = X * X ;
X22 = X * X ;
X32 = X * X ;
X_01 = X * X;
X_02 = X * X;
X_03 = X * X;
X_12 = X * X;
X_13 = X * X;
X_23 = X * X;
dcm = 1.0 - 2.0 * (X22+X32);
dcm = 2.0 * (X_12 +X_03 );
dcm = 2.0 * (X_13 -X_02 );
dcm = 2.0 * (X_12 -X_03 );
dcm = 1.0 - 2.0 * (X12+X32);
dcm = 2.0 * (X_23 +X_01 );
dcm = 2.0 * (X_13 +X_02 );
dcm = 2.0 * (X_23 -X_01 );
dcm = 1.0 - 2.0 * (X12+X22);
H_tmp= 2.0 / ( (dcm * dcm) + (dcm * dcm) );
H_tmp= -1.0 / SquareRootFloat( 1.0 - (dcm * dcm));
H_tmp= 2.0 / ( (dcm * dcm) + (dcm * dcm) );
H = H_tmp * ( X * dcm );
H = H_tmp * ( X * dcm + 2.0 * X * dcm );
H = H_tmp * ( X * dcm + 2.0 * X * dcm );
H = H_tmp * ( X * dcm );
H = H_tmp * (-2.0) * X;
H = H_tmp * ( 2.0 )* X;
H = H_tmp * (-2.0 )* X;
H = H_tmp * ( 2.0 )* X;
H = H_tmp * ( X * dcm );
H = H_tmp * ( X * dcm );
H = H_tmp * ( X * dcm + 2.0 * X * dcm );
H = H_tmp * ( X * dcm + 2.0 * X * dcm );
}
/*******************************************************************************
* Function Name: E_kalmanfilter
* Description : 扩展卡尔曼滤波器的更新过程
* Input : null(所需矩阵均为全局变量)
* Output : null(计算结果均为全局变量)
* Return : null
*******************************************************************************/
void E_kalmanfilter(void)
{
float P_tmp1,P_tmp2;
float H_tmp,K_tmp1,K_tmp2;
float X_tmp1;
//初始化得到A;
A_Maxup(0.01,1.0,0.5);
//1. X_tmp=A*X
Matrix_mul(4,4,1,*A,X,X_tmp);
//2. P_tmp=A*P*A^+Q
Matrix_mul(4,4,4,*A,*P,*P_tmp1);
Matrix_transpose(4,4,*A,*P_tmp2);
Matrix_mul(4,4,4,*P_tmp1,*P_tmp2,*P_tmp);
Matrix_add(4,4,*P_tmp,*Q,*P_tmp);
//插入H阵的更新,因为非线性滤波，观测矩阵H由X实时变化得到；
H_Maxup(X,H);
//3. K = P_tmp*H^*(H*P_tmp*H^+R)^(-1)
Matrix_transpose(3,4,*H,*H_tmp);
Matrix_mul(4,4,3,*P,*H_tmp,*K_tmp1);
Matrix_mul(3,4,3,*H,*K_tmp1,*K_tmp2);
Matrix_add(3,3,*K_tmp2,*R,*K_tmp2);
Matrix_inverse(3,3,K_tmp2,K_tmp2);
Matrix_mul(4,3,3,*K_tmp1,*K_tmp2,*K);
//4. Z_tmp =Z - H*X_tmp
Matrix_mul(3,4,1,*H,X_tmp,Z_tmp);
Matrix_sub(3,1,Z,Z_tmp,Z_tmp);
//5. X = X_tmp + K*Z_tmp
Matrix_mul(4,3,1,*K,Z_tmp,X_tmp1);
Matrix_add(4,3,X_tmp,X_tmp1,X);
//6. P = (I - K*H)P_tmp
Matrix_mul(4,3,4,*K,*H,*P_tmp1);
Matrix_sub(4,4,*I,*P_tmp1,*P_tmp2);
Matrix_mul(4,4,4,*P_tmp2,*P_tmp,*P);
}
小弟很久之前看过关于四元数和EKF的融合代码，但后来因为某些事情忘记了。最近把之前找到资料上的代码弄了出来，愣是搞不定Z矩阵是什么数据了。。。带入加速度传感器，和磁敏传感器的角度Roll，Pitch，Yaw后滤波器算出的角度是不对的，或者说差距很大（相应的轴数据没变，另两个轴，数据变得一头劲，要么都不变）。然后重新到网上搜资料也搜不到了。。。这个算法就此消声觅迹了么？求指点啊。。。 帮顶！！！ 召唤知情人士。。。 看了半天暂时没结果……
卡在没搞明白H_Maxup那个函数里的H_tmp向量是什么含义 K.O.Carnivist 发表于 2012-12-27 21:42 static/image/common/back.gif
看了半天暂时没结果……
卡在没搞明白H_Maxup那个函数里的H_tmp向量是什么含义 ...
那个应该是求导后的分母。。。我还是搞不清楚这个算法中Z应该带入什么进去。。。继续召唤前辈。。。 好像是以四元数为状态量的EKF 观测量上面看不出来 不过应该是加表或者磁 楼主后来整明白了没有？ 看着就头大
页:
[1]