Open TianzeXie opened 8 months ago
The equation on the website should be correct as this is a linearization equation. We do not know the true state, so it is not possible to evaluate the equation you have posted. Thus we linearize the true equations z = h(x) + n
to get our z = h(x_hat) + H (x - x_hat) + n
. The key to FEJ is about what point to evaluate the Jacobian at the ensure correct observability properties. Hope this helps.
true, just like you said, the ture equation is : $z_{k}=h(xk)+n$. In my context, it is $z{k+1}=h(x{k+1})+n$, so I have my first equation: $\tilde{z{k+1}}=z{k+1}-h(x{k+1})$, right? And then consider the FEJ, I linearize it at the point $\hat{x_h}$, so I get my second equation.
Typically the residual is defined after linearization. The third equation you wrote is just directly equal to noise not the residual.
in this page https://docs.openvins.com/fej.html At the last equation ,should it be $$\tilde{z{k+1}}= z{k+1}-h(x_{k+1})=\bar{Hk}(x{k+1}-\hat{x}_k)$$ Because every time the filter update, we assume that the robot is at the true state of the k+1 timestamp where it observe the features.