# SLAM Initialize new landmark - specifically the covariance

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;