q = rand(6,1);
qd = rand(6,1);
q2d = rand(6,1);
Yf = full_regressor(q,qd,q2d);
Yb = base_regressor(q,qd,q2d);
Yb2 = Yf*Pb';
norm(Yb2 - Yb)