0
$\begingroup$

Im trying to bring an EKF - SLAM to life but it keeps on failing. Specifially, I think, my mistake lies in the initialization of the new cone <-> covariance. I multiply the Jacobian (derived with the robots specific variables [x,y,theta] with the Crosscovariance robot <-> cone of this specific cone. I repeat this step for all the cones which got measured before the one im initiating right now. Is this the right way to "fill" the whole row (besides from the robot <-> new cone covariances)

These are the Jacobians: J_h_inv = [1 0 -sin(angle+theta_robot)*distance ; 0 1 cos(angle +theta_robot)*distance]; J_z = [cos(angle+theta_robot) -sin(angle+theta_robot)*distance ;sin(angle+theta_robot) cos(angle+theta_robot)*distance];

matrices=[J_h_inv J_z];

Also because of the negative sin within the jacobians the landmarks get initialized negativly which cant be correct, right? But the Jacobians are the same as in "SLAM for dummies", Thrun, "Ekf A very quick guide" etc...

This is the specific part of my code:

        P_zero = zeros(length(x_local));                        %Creating a broader matrix
        P_zero(1:length(P_local),1:length(P_local)) = P_local;  %Filling in the old one
        
        %Add new variance Cone 
        P_zero(end-1:end,end-1:end) = Jacobians_new_covariance(:,1:3)*P_local(1:3,1:3)*Jacobians_new_covariance(:,1:3)'+Jacobians_new_covariance(:,4:5)*R_local*Jacobians_new_covariance(:,4:5)';
        % in short -> Jac_v*P_local*Jac_v'+Jac_c*R_local*Jac_c'
        
        %Add new Cone <-> Cone Covariance
            for t = 1:length(cone_positions_cell{1,1}(:,1))-1 % Cone_positions_cell{1,1} just counts the number of cones. Thats why it´s -1 not 2
                
            P_cross_cones = Jacobians_new_covariance(:,1:3)*P_local((2+2*t):(3+2*t),1:3)';  %Is this the right formula?!
            P_zero(2+2*t:3+2*t,end-1:end) = P_cross_cones';
            P_zero(end-1:end,2+2*t:3+2*t) = P_cross_cones;
            end
            
        %Add covariance vehicle <-> new cone   
        P_cross_vehicle_new = Jacobians_new_covariance(:,1:3)*P_local(1:3,1:3); 
        P_zero(end-1:end,1:3) = P_cross_vehicle_new;
        P_zero(1:3,end-1:end) = P_cross_vehicle_new';

        P_local = P_zero;
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.