Extending the code to 3 dimensions

[prathyush90]

Hi I have tried extending your code to 3 dimension coordinates.But the determinant of covariance is negative and it affects the whole weight calculations changing them to NaN

Jacobians : Predicted is same [[distance],[angle]]
Considering the angle between two vectors can be expressed as R**2 cos(angle) = A.B
I was able to calculate the jacobians as

feature jacobian : [ (dx/distance), (dy/distance), (dz/distance), feature_alpha, feature_beta, feature_gamma]

commonmultiple   = (-1 / (sqrt(1-pow(costheta,2) + 0.00000001)));
feature_alpha        = (particlePos[0] * commonmultiple) / (costhetadenom)
feature_beta         = (particlePos[1] * commonmultiple) / (costhetadenom)
feature_gamma    = (particlePos[2] * commonmultiple) / (costhetadenom) 

Can you help me where i'm going wrong.Are my feature jacobian calculations wrong?

[nwang57]

As far as I know, the jabobian really depends on how you represent your robot pose in 3d space. I believe thats where the thing goes wrong. A way to represent the pose is using (x,y,z, theta, tau). x,y,z are used to represent the translation and the two angel parameter is used to represent the orientation of the robot. The jacobian of this is more complex than the 2d case.