syms q1 q2 q3 q4 gx gy gz ax ay az mx my mz bx bz by h hx hy dx dy dz sx sy sz
q = [q1 q2 q3 q4];
cong_q = [q1 -q2 -q3 -q4];
gyro = [0 gx gy gz];
a = [0 ax ay az];
b = [0 bx 0 bz];
d = [0 dx dy dz];
s = [0 sx sy sz];
m = [0 mx my mz];
wg = [0 0 0 1];
qdotg = quatmul(q,gyro);
fg1 = quatmul(cong_q,wg);
fg = quatmul(fg1,q) - transpose(a);
fg(1) = [];
Jg = jacobian(fg, [q1, q2, q3, q4]);
fb = quatmul(cong_q,b);
fb = quatmul(fb,q) - transpose(m);
fb(1) = [];
Jb = jacobian(fb, [q1, q2, q3, q4]);
Jg_t_fg = transpose(Jg)*fg;
Jb_t_fb = transpose(Jb)*fb;
ftot = Jg_t_fg + Jb_t_fb;
fnorm = ftot * (1/sqrt(ftot(1)^2+ftot(2)^2+ftot(3)^2+ftot(4)^2));
function quatprod = quatmul(r, k)
quatprod1 = r(1)*k(1) - r(2)*k(2) - r(3)*k(3) - r(4)*k(4);
quatprod2 = r(1)*k(2) + r(2)*k(1) + r(3)*k(4) - r(4)*k(3);
quatprod3 = r(1)*k(3) - r(2)*k(4) + r(3)*k(1) + r(4)*k(2);
quatprod4 = r(1)*k(4) + r(2)*k(3) - r(3)*k(2) + r(4)*k(1);
quatprod = [quatprod1;quatprod2;quatprod3;quatprod4];
end