/*!
void Rotate(number x, number y, number z, number w)\n
Set the bone rotation to the quaternion x;y;z;w. w is the realpart
*/
void Item_bone::Rotation(float a, float b , float c, float d){
float q[4] = {a,b,c,d};
float inv[4] = {-quat[0],-quat[1],-quat[2],quat[3]};
qmult(inv,q,q);
qmult(quat,q,quat);
for (QTreeWidgetItemIterator it(this);*it;it++){
if (*it == this){
qDebug() << "iterator this";
continue;
}
for(QTreeWidgetItem *i = *it;dynamic_cast<Item_bone*>(i);i = i->parent()){
if (i == this) if(Item_bone* bone = dynamic_cast<Item_bone*>(*it)){
qmult(bone->quat, q, bone->quat);
qrotaround(bone->joint, joint, q, bone->joint);
qDebug() << bone->QTreeWidgetItem::text(0);
qDebug() << bone->quat[0] << bone->quat[1] << bone->quat[2] << bone->quat[3];
qDebug() << bone->joint[0] << bone->joint[1] << bone->joint[2] << "\n";
}
}
}
}
result_type operator()(A0& out, const A1& in) const
{
BOOST_AUTO_TPL(x,boost::proto::child_c<0>(in));
std::size_t k = boost::proto::child_c<1>(in);
std::size_t m = boost::proto::child_c<2>(in);
std::size_t n = numel(x);
BOOST_AUTO_TPL(a, nt2::expand(nt2::from_diag(x), nt2::of_size(m, n)));
table<value_t> aa;
if (!k)
{
// A = U*A*V where U and V are two random orthogonal matrices.
aa = nt2::transpose(nt2::qmult(nt2::transpose(qmult(a))));
}
else
{
aa = a;
}
table<value_t> suma = nt2::sum(nt2::sqr(aa));
do
{
nt2::table<ptrdiff_t> y = nt2::find(nt2::lt(suma, nt2::One<value_t>()));
nt2::table<ptrdiff_t> z = nt2::find(nt2::gt(suma, nt2::One<value_t>()));
if(nt2::isempty(y) || nt2::isempty(z)) break;
std::size_t i = y(nt2::iceil(nt2::rand(nt2::meta::as_<value_t>())*nt2::numel(y)));
std::size_t j = z(nt2::iceil(nt2::rand(nt2::meta::as_<value_t>())*nt2::numel(z)));
if (i > j) std::swap(i, j);
value_t aij = nt2::dot(aa(nt2::_, i), aa(nt2::_, j));
value_t alpha = nt2::sqrt(nt2::sqr(aij)- nt2::minusone(suma(i))*nt2::minusone(suma(j)));
value_t t1 = (aij + nt2::signnz(aij)*alpha)/nt2::minusone(suma(j));
value_t t2 = nt2::oneminus(suma(i))/(nt2::oneminus(suma(j))*t1);
value_t t = (nt2::rand(nt2::meta::as_<value_t>()) > Half<value_t>()) ? t1 : t2;
value_t c = nt2::rsqrt(nt2::oneplus(nt2::sqr(t))) ;
value_t s = t*c ;
BOOST_AUTO_TPL( ij, nt2::cath(i, j));
aa(nt2::_,ij) = nt2::mtimes(aa(nt2::_,ij), nt2::catv(nt2::cath(c, s), nt2::cath(-s, c))) ;
suma(j) += nt2::minusone(suma(i));
suma(i) = nt2::One<value_t>();
} while (true);
out = aa;
return out;
}
// Main get_nav filter function
void get_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
double tnow, imu_dt;
double dq[4], quat_new[4];
// compute time-elapsed 'dt'
// This compute the navigation state at the DAQ's Time Stamp
tnow = sensorData_ptr->imuData_ptr->time;
imu_dt = tnow - tprev;
tprev = tnow;
// ================== Time Update ===================
// Temporary storage in Matrix form
quat[0] = navData_ptr->quat[0];
quat[1] = navData_ptr->quat[1];
quat[2] = navData_ptr->quat[2];
quat[3] = navData_ptr->quat[3];
a_temp31[0][0] = navData_ptr->vn; a_temp31[1][0] = navData_ptr->ve;
a_temp31[2][0] = navData_ptr->vd;
b_temp31[0][0] = navData_ptr->lat; b_temp31[1][0] = navData_ptr->lon;
b_temp31[2][0] = navData_ptr->alt;
// AHRS Transformations
C_N2B = quat2dcm(quat, C_N2B);
C_B2N = mat_tran(C_N2B, C_B2N);
// Attitude Update
// ... Calculate Navigation Rate
nr = navrate(a_temp31,b_temp31,nr);
dq[0] = 1;
dq[1] = 0.5*om_ib[0][0]*imu_dt;
dq[2] = 0.5*om_ib[1][0]*imu_dt;
dq[3] = 0.5*om_ib[2][0]*imu_dt;
qmult(quat,dq,quat_new);
quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
if(quat[0] < 0) {
// Avoid quaternion flips sign
quat[0] = -quat[0];
quat[1] = -quat[1];
quat[2] = -quat[2];
quat[3] = -quat[3];
}
navData_ptr->quat[0] = quat[0];
navData_ptr->quat[1] = quat[1];
navData_ptr->quat[2] = quat[2];
navData_ptr->quat[3] = quat[3];
quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
// Velocity Update
dx = mat_mul(C_B2N,f_b,dx);
dx = mat_add(dx,grav,dx);
navData_ptr->vn += imu_dt*dx[0][0];
navData_ptr->ve += imu_dt*dx[1][0];
navData_ptr->vd += imu_dt*dx[2][0];
// Position Update
dx = llarate(a_temp31,b_temp31,dx);
navData_ptr->lat += imu_dt*dx[0][0];
navData_ptr->lon += imu_dt*dx[1][0];
navData_ptr->alt += imu_dt*dx[2][0];
// JACOBIAN
F = mat_fill(F, ZERO_MATRIX);
// ... pos2gs
F[0][3] = 1.0; F[1][4] = 1.0; F[2][5] = 1.0;
// ... gs2pos
F[5][2] = -2*g/EARTH_RADIUS;
// ... gs2att
temp33 = sk(f_b,temp33);
atemp33 = mat_mul(C_B2N,temp33,atemp33);
F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2];
F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2];
F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2];
// ... gs2acc
F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2];
F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2];
F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2];
// ... att2att
temp33 = sk(om_ib,temp33);
F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2];
F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2];
F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2];
// ... att2gyr
F[6][12] = -0.5;
F[7][13] = -0.5;
F[8][14] = -0.5;
// ... Accel Markov Bias
F[9][9] = -1.0/TAU_A; F[10][10] = -1.0/TAU_A; F[11][11] = -1.0/TAU_A;
F[12][12] = -1.0/TAU_G; F[13][13] = -1.0/TAU_G; F[14][14] = -1.0/TAU_G;
//fprintf(stderr,"Jacobian Created\n");
// State Transition Matrix: PHI = I15 + F*dt;
temp1515 = mat_scalMul(F,imu_dt,temp1515);
PHI = mat_add(I15,temp1515,PHI);
// Process Noise
G = mat_fill(G, ZERO_MATRIX);
G[3][0] = -C_B2N[0][0]; G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2];
G[4][0] = -C_B2N[1][0]; G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2];
G[5][0] = -C_B2N[2][0]; G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2];
G[6][3] = -0.5;
G[7][4] = -0.5;
G[8][5] = -0.5;
G[9][6] = 1.0; G[10][7] = 1.0; G[11][8] = 1.0;
G[12][9] = 1.0; G[13][10] = 1.0; G[14][11] = 1.0;
//fprintf(stderr,"Process Noise Matrix G is created\n");
// Discrete Process Noise
temp1512 = mat_mul(G,Rw,temp1512);
temp1515 = mat_transmul(temp1512,G,temp1515); // Qw = G*Rw*G'
Qw = mat_scalMul(temp1515,imu_dt,Qw); // Qw = dt*G*Rw*G'
Q = mat_mul(PHI,Qw,Q); // Q = (I+F*dt)*Qw
temp1515 = mat_tran(Q,temp1515);
Q = mat_add(Q,temp1515,Q);
Q = mat_scalMul(Q,0.5,Q); // Q = 0.5*(Q+Q')
//fprintf(stderr,"Discrete Process Noise is created\n");
// Covariance Time Update
temp1515 = mat_mul(PHI,P,temp1515);
P = mat_transmul(temp1515,PHI,P); // P = PHI*P*PHI'
P = mat_add(P,Q,P); // P = PHI*P*PHI' + Q
temp1515 = mat_tran(P, temp1515);
P = mat_add(P,temp1515,P);
P = mat_scalMul(P,0.5,P); // P = 0.5*(P+P')
//fprintf(stderr,"Covariance Updated through Time Update\n");
navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
navData_ptr->err_type = TU_only;
//fprintf(stderr,"Time Update Done\n");
// ================== DONE TU ===================
if(sensorData_ptr->gpsData_ptr->newData){
// ================== GPS Update ===================
sensorData_ptr->gpsData_ptr->newData = 0; // Reset the flag
// Position, converted to NED
a_temp31[0][0] = navData_ptr->lat;
a_temp31[1][0] = navData_ptr->lon; a_temp31[2][0] = navData_ptr->alt;
pos_ins_ecef = lla2ecef(a_temp31,pos_ins_ecef);
a_temp31[2][0] = 0.0;
//pos_ref = lla2ecef(a_temp31,pos_ref);
pos_ref = mat_copy(a_temp31,pos_ref);
pos_ins_ned = ecef2ned(pos_ins_ecef,pos_ins_ned,pos_ref);
pos_gps[0][0] = sensorData_ptr->gpsData_ptr->lat*D2R;
pos_gps[1][0] = sensorData_ptr->gpsData_ptr->lon*D2R;
pos_gps[2][0] = sensorData_ptr->gpsData_ptr->alt;
pos_gps_ecef = lla2ecef(pos_gps,pos_gps_ecef);
pos_gps_ned = ecef2ned(pos_gps_ecef,pos_gps_ned,pos_ref);
// Create Measurement: y
y[0][0] = pos_gps_ned[0][0] - pos_ins_ned[0][0];
y[1][0] = pos_gps_ned[1][0] - pos_ins_ned[1][0];
y[2][0] = pos_gps_ned[2][0] - pos_ins_ned[2][0];
y[3][0] = sensorData_ptr->gpsData_ptr->vn - navData_ptr->vn;
y[4][0] = sensorData_ptr->gpsData_ptr->ve - navData_ptr->ve;
y[5][0] = sensorData_ptr->gpsData_ptr->vd - navData_ptr->vd;
//fprintf(stderr,"Measurement Matrix, y, created\n");
// Kalman Gain
temp615 = mat_mul(H,P,temp615);
temp66 = mat_transmul(temp615,H,temp66);
atemp66 = mat_add(temp66,R,atemp66);
temp66 = mat_inv(atemp66,temp66); // temp66 = inv(H*P*H'+R)
//fprintf(stderr,"inv(H*P*H'+R) Computed\n");
temp156 = mat_transmul(P,H,temp156); // P*H'
//fprintf(stderr,"P*H' Computed\n");
K = mat_mul(temp156,temp66,K); // K = P*H'*inv(H*P*H'+R)
//fprintf(stderr,"Kalman Gain Computed\n");
// Covariance Update
temp1515 = mat_mul(K,H,temp1515);
ImKH = mat_sub(I15,temp1515,ImKH); // ImKH = I - K*H
temp615 = mat_transmul(R,K,temp615);
KRKt = mat_mul(K,temp615,KRKt); // KRKt = K*R*K'
temp1515 = mat_transmul(P,ImKH,temp1515);
P = mat_mul(ImKH,temp1515,P); // ImKH*P*ImKH'
temp1515 = mat_add(P,KRKt,temp1515);
P = mat_copy(temp1515,P); // P = ImKH*P*ImKH' + KRKt
//fprintf(stderr,"Covariance Updated through GPS Update\n");
navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
// State Update
x = mat_mul(K,y,x);
denom = (1.0 - (ECC2 * sin(navData_ptr->lat) * sin(navData_ptr->lat)));
denom = sqrt(denom*denom);
Re = EARTH_RADIUS / sqrt(denom);
Rn = EARTH_RADIUS*(1-ECC2) / denom*sqrt(denom);
navData_ptr->alt = navData_ptr->alt - x[2][0];
navData_ptr->lat = navData_ptr->lat + x[0][0]/(Re + navData_ptr->alt);
navData_ptr->lon = navData_ptr->lon + x[1][0]/(Rn + navData_ptr->alt)/cos(navData_ptr->lat);
navData_ptr->vn = navData_ptr->vn + x[3][0];
navData_ptr->ve = navData_ptr->ve + x[4][0];
navData_ptr->vd = navData_ptr->vd + x[5][0];
quat[0] = navData_ptr->quat[0];
quat[1] = navData_ptr->quat[1];
quat[2] = navData_ptr->quat[2];
quat[3] = navData_ptr->quat[3];
// Attitude correction
dq[0] = 1.0;
dq[1] = x[6][0];
dq[2] = x[7][0];
dq[3] = x[8][0];
qmult(quat,dq,quat_new);
quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
navData_ptr->quat[0] = quat[0];
navData_ptr->quat[1] = quat[1];
navData_ptr->quat[2] = quat[2];
navData_ptr->quat[3] = quat[3];
quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
navData_ptr->ab[0] = navData_ptr->ab[0] + x[9][0];
navData_ptr->ab[1] = navData_ptr->ab[1] + x[10][0];
navData_ptr->ab[2] = navData_ptr->ab[2] + x[11][0];
navData_ptr->gb[0] = navData_ptr->gb[0] + x[12][0];
navData_ptr->gb[1] = navData_ptr->gb[1] + x[13][0];
navData_ptr->gb[2] = navData_ptr->gb[2] + x[14][0];
navData_ptr->err_type = gps_aided;
//fprintf(stderr,"Measurement Update Done\n");
}
// Remove current estimated biases from rate gyro and accels
sensorData_ptr->imuData_ptr->p -= navData_ptr->gb[0];
sensorData_ptr->imuData_ptr->q -= navData_ptr->gb[1];
sensorData_ptr->imuData_ptr->r -= navData_ptr->gb[2];
sensorData_ptr->imuData_ptr->ax -= navData_ptr->ab[0];
sensorData_ptr->imuData_ptr->ay -= navData_ptr->ab[1];
sensorData_ptr->imuData_ptr->az -= navData_ptr->ab[2];
// Get the new Specific forces and Rotation Rate,
// use in the next time update
f_b[0][0] = sensorData_ptr->imuData_ptr->ax;
f_b[1][0] = sensorData_ptr->imuData_ptr->ay;
f_b[2][0] = sensorData_ptr->imuData_ptr->az;
om_ib[0][0] = sensorData_ptr->imuData_ptr->p;
om_ib[1][0] = sensorData_ptr->imuData_ptr->q;
om_ib[2][0] = sensorData_ptr->imuData_ptr->r;
}
