|
发表于 2012-8-2 00:00:20
|
显示全部楼层
js200300953 发表于 2012-8-1 21:07
对
还是存在不少问题,先上个代码``````
void Gradient_Descent()
{
float Normalize;
float X_Delta_a,Y_Delta_a,Z_Delta_a;
float X_Delta_h,Y_Delta_h,Z_Delta_h;
float txz,twy,tyz,twx,tx2,ty2,txy,twz,tz2;
float df_dw,df_dx,df_dy,df_dz;
float yHw,yHx,yHy,yHz,zHw,zHx,zHy,zHz;
txy = 2*Quaternion.Ax*Quaternion.Ay;
twz = 2*Quaternion.Aw*Quaternion.Az;
txz = 2*Quaternion.Ax*Quaternion.Az;
twy = 2*Quaternion.Aw*Quaternion.Ay;
tyz = 2*Quaternion.Ay*Quaternion.Az;
twx = 2*Quaternion.Aw*Quaternion.Ax;
tx2 = 2*Quaternion.Ax*Quaternion.Ax;
ty2 = 2*Quaternion.Ay*Quaternion.Ay;
tz2 = 2*Quaternion.Az*Quaternion.Az;
yHw = HMC_Y_W*Quaternion.Aw;
yHx = HMC_Y_W*Quaternion.Ax;
yHy = HMC_Y_W*Quaternion.Ay;
yHz = HMC_Y_W*Quaternion.Az;
zHw = HMC_Z_W*Quaternion.Aw;
zHx = HMC_Z_W*Quaternion.Ax;
zHy = HMC_Z_W*Quaternion.Ay;
zHz = HMC_Z_W*Quaternion.Az;
//Δa Gravity_X为测量值
X_Delta_a = txz-twy-Gravity_X;
Y_Delta_a = tyz+twx-Gravity_Y;
Z_Delta_a = 1-tx2-ty2-Gravity_Z;
//Δh
X_Delta_h = HMC_Y_W*(txy+twz)+HMC_Z_W*(txz-twy)-HMC_X;
Y_Delta_h = HMC_Y_W*(1-tx2-tz2)+HMC_Z_W*(tyz+twx)-HMC_Y;
Z_Delta_h = HMC_Y_W*(tyz-twx)+HMC_Z_W*(1-tx2-ty2)-HMC_Z;
df_dw = -4*X_Delta_a*Quaternion.Ay+4*Y_Delta_a*Quaternion.Ax+4*X_Delta_h*(yHz-zHy)+4*Y_Delta_h*zHx-4*Z_Delta_h*yHx;
df_dx = 4*X_Delta_a*Quaternion.Az+4*Y_Delta_a*Quaternion.Aw-4*Z_Delta_a*Quaternion.Ax+4*X_Delta_h*(yHy-zHz)+4*Y_Delta_h*(zHw-yHx)-4*Z_Delta_h*(yHw-zHx);
df_dy = -4*X_Delta_a*Quaternion.Aw+4*Y_Delta_a*Quaternion.Az-4*Z_Delta_a*Quaternion.Ay+4*X_Delta_h*(yHx-zHw)+4*Y_Delta_h*zHz+4*Z_Delta_h*(yHz-zHy);
df_dz = 4*X_Delta_a*Quaternion.Ax+4*Y_Delta_a*Quaternion.Ay+4*X_Delta_h*(yHw+zHx)+4*Y_Delta_h*(zHy-yHz)+4*Z_Delta_h*yHy;
Quaternion.Aw+=r*df_dw;
Quaternion.Ax+=r*df_dx;
Quaternion.Ay+=r*df_dy;
Quaternion.Az+=r*df_dz;
Normalize = Q_rsqrt(Quaternion.Aw*Quaternion.Aw+Quaternion.Ax*Quaternion.Ax+Quaternion.Ay*Quaternion.Ay+Quaternion.Az*Quaternion.Az);
Quaternion.Aw = Quaternion.Aw*Normalize;
Quaternion.Ax = Quaternion.Ax*Normalize;
Quaternion.Ay = Quaternion.Ay*Normalize;
Quaternion.Az = Quaternion.Az*Normalize;
}
我觉得我对这个方法的理解上还是存在一定的误区,我先说说目前我自己的想法,如果我想先不理电子罗盘的数据,先用加速度传感器修正X,Y轴向的飘移,那么,最终得到的偏导公式是否为
// df_dw = -4*X_Delta_a*Quaternion.Ay+4*Y_Delta_a*Quaternion.Ax;
// df_dx = 4*X_Delta_a*Quaternion.Az+4*Y_Delta_a*Quaternion.Aw-4*Z_Delta_a*Quaternion.Ax;
// df_dy = -4*X_Delta_a*Quaternion.Aw+4*Y_Delta_a*Quaternion.Az-4*Z_Delta_a*Quaternion.Ay;
// df_dz = 4*X_Delta_a*Quaternion.Ax+4*Y_Delta_a*Quaternion.Ay; 这样子???
这样子理解有没有问题? |
|