Hi,
the cost function you are describing doesn’t fit into the nonlinear least squares form.
I suggest using the external cost module and implement what you denote as y_ref_quat as a parameter.
Cheers!
1 Like