UKF Prediction Step with Sigma Points [robot_localization]
Hi Community,
this question is not directly related to ROS but as i used robot_localization as a package to check my implementation i will ask here. And robot_localization does a very specific thing which i am not sure why. Assuming:
x_k+1 = f(x_k)
I generate Sigma points as usual which are matching the sigma points generate from robot_localization.
Now i put each sigma point into the transfer function to predict this sigma point.
In robot_localization they calculate the transfer function for the last state and reuse this for each of the sigma points.
This does make a difference due to the non linear model (the transfer function, for example from Vx to Z is dependent on the orientation).
My example was that the current state does have roll, pitchand yaw equal 0, and i have a non zero velocity.
If i now compute the transfer function i from Vx to Z the component will be 0.
If now one of my Sigma Points has a non zero pitch and i reuse the transfer function which was calculated for the current state my predicted sigma point would have the same Zcomponent as the predicted mean state (assuming all other Zinfluences are zero). If i would use the Sigma Point itself to recompute the transfer function from Vx to Z the component will be non zero and my predicted Sigma Point would also have a non zero Zcomponent.
First question is which of these is the correct way to do it?
Secondly after prediction of the sigma points we calculate the mean again. What now drove my crazy is that with my interpretation (calculating transfer function based on the sigma point) the mean does not represent the expected mean it always is smaller / more negative than the predicted state. If i am using the way robot_localization is using the mean is as it is expected.
This is also only true if lambda is negative and therefore the sigma weights to create the mean are these non intuitive numbers like -999999 for the mean and 33333 for all the other sigma points.
I think due to the very high multiplication weights a small change in the predicted sigma point does have a huge impact on the output.
Did someone face this issue beforehand or has more understanding of UKF to maybe help in this regard?
Greetings Sven
