I have some trouble deriving on of the error jacobians in the dynamic initialization. In particular, the jacobian of the error w.r.t rotation component of the state as coded here. It says,
H_theta = -Jr_inv * R_lin.transpose();
From your technical report, the error is defined as global error $^G {\delta\theta}$. The state to be estimated is $^I_G{R}$. So the perturbations are right-sided. Is that correct?
Now $^G{\delta\theta} = \log(^I_G{R}^T{^IG{R}\textrm{lin}})$. So the jacobian w.r.t the state $^I_G{R}$ should be the following through chain-rule.
Which should be (switching back to code for brevity),
Jr_inv * R_lin.transpose() * (-R_i)
Where we are using the following for elementary jacobians. While they can be derived through perturbing the corresponding expressions on the right, I will refer to Eqn 140 and 141 in Sola et al.
$J^{QR}_Q = R^T$
$J^{R^T}_{R} = -R$
So finally I have,
H_theta = - Jr_inv * R_lin.transpose() * R_i
which differs from the code by having an extra R_i on the right.
What am I missing in my understanding? I did find and look at 276 but that didn't quite address it fully.
To the authors and the community,
I have some trouble deriving on of the error jacobians in the dynamic initialization. In particular, the jacobian of the error w.r.t rotation component of the state as coded here. It says,
From your technical report, the error is defined as global error $^G {\delta\theta}$. The state to be estimated is $^I_G{R}$. So the perturbations are right-sided. Is that correct?
Now $^G{\delta\theta} = \log(^I_G{R}^T{^IG{R}\textrm{lin}})$. So the jacobian w.r.t the state $^I_G{R}$ should be the following through chain-rule.
$\frac{\partial \log{^G{\delta\theta}}}{\partial ^G{\delta\theta}} \times \frac{\partial ^I_G{R}^T{^IG{R}\textrm{lin}}}{\partial ^I_G{R}^T} \times \frac{\partial ^I_G{R}^T}{\partial ^I_G{R}}$
Which should be (switching back to code for brevity),
Where we are using the following for elementary jacobians. While they can be derived through perturbing the corresponding expressions on the right, I will refer to Eqn 140 and 141 in Sola et al.
So finally I have,
which differs from the code by having an extra
R_i
on the right.What am I missing in my understanding? I did find and look at 276 but that didn't quite address it fully.