function_inital_jacobian.m

卡尔曼滤波估计雅可比的matlab代码,用于计算机视觉的在线估计

function J_0 = Function_Inital_Jacobian(sita,x_y_z);
% sita=[0;pi/2];x_y_z=[0.8,0.5,0];
yo=x_y_z;

de_sita1=[0.1;0];
sita1=sita+de_sita1;
fe1=Function_image_feature(sita1,yo)-Function_image_feature(sita,yo);

de_sita2=[0;0.1];
sita2=sita+de_sita2;
fe2=Function_image_feature(sita2,yo)-Function_image_feature(sita,yo);

J_0=[[fe1./de_sita1(1)]',[fe2./de_sita2(2)]'];