包含了ekfSLAM 1.0版本和2.0版本的MATLAB仿真程序。

包含了ekfSLAM 1.0版本和2.0版本的MATLAB仿真程序。包含了ekfSLAM 1.0版本和2.0版本的MATLAB仿真程序。

%-----------------------------------------------------------------------
% 1-point RANSAC EKF SLAM from a monocular sequence
%-----------------------------------------------------------------------

% Copyright （C） 2010 Javier Civera and J. M. M. Montiel
% Universidad de Zaragoza Zaragoza Spain.

% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation. Read http://www.gnu.org/copyleft/gpl.html for details

% If you use this code for academic work please reference:
%   Javier Civera Oscar G. Grasa Andrew J. Davison J. M. M. Montiel
%   1-Point RANSAC for EKF Filtering: Application to Real-Time Structure from Motion and Visual Odometry
%   to appear in Journal of Field Robotics October 2010.

%-----------------------------------------------------------------------
% Authors:  Javier Civera -- jcivera@unizar.es 
%           J. M. M. Montiel -- josemari@unizar.es

% Robotics Perception and Real Time Group
% Arag髇 Institute of Engineering Research （I3A）
% Universidad de Zaragoza 50018 Zaragoza Spain
% Date   :  May 2010
%-----------------------------------------------------------------------

function P_RES = add_a_feature_covariance_inverse_depth（ P uvd Xv std_pxl std_rho cam ）

fku =  cam.K（11）;
fkv =  cam.K（22）;
U0  =  cam.K（13）;
V0  =  cam.K（23）;

q_wc = Xv（4:7）;
R_wc = q2r（q_wc）;

uvu = undistort_fm（ uvd cam ）;
uu = uvu（1）;
vu = uvu（2）;

X_c = -（U0-uu）/fku;
Y_c = -（V0-vu）/fkv;
Z_c = 1;

XYZ_c = [X_c; Y_c; Z_c];

XYZ_w = R_wc*XYZ_c;
X_w = XYZ_w（1）;
Y_w = XYZ_w（2）;
Z_w = XYZ_w（3）;

% Derivatives

dtheta_dgw = [ Z_w/（X_w^2+Z_w^2） 0 -X_w/（X_w^2+Z_w^2） ];
dphi_dgw = [ （X_w*Y_w）/（（X_w^2+Y_w^2+Z_w^2）*sqrt（X_w^2+Z_w^2））...
        -sqrt（X_w^2+Z_w^2）/（X_w^2+Y_w^2+Z_w^2） （Z_w*Y_w）/（（X_w^2+Y_w^2+Z_w^2）*sqrt（X_w^2+Z_w^2）） ];
dgw_dqwr = dRq_times_a_by_dq（ q_wc XYZ_c ）;

dtheta_dqwr = dtheta_dgw*dgw_dqwr;
dphi_dqwr = dphi_dgw*dgw_dqwr;
dy_dqwr = sparse（ [ zeros（34）; dtheta_dqwr; dphi_dqwr; zeros（14） ] ）;
dy_drw = sparse（ [ eye（3）; zeros（33） ] ）;

dy_dxv = sparse（ [ dy_drw dy_dqwr zeros（66） ] ）;

dyprima_dgw = sparse（ [ zeros（33）; dtheta_dgw; dphi_dgw ] ）;
dgw_dgc = R_wc;
dgc_dhu = sparse（ [ +1/fku      0       0;
                0   +1/fkv      0]‘ ）;
dhu_dhd = jacob_undistor_fm（ cam  uvd ）;

dyprima_dhd = dyprima_dgw*dgw_dgc*dgc_dhu*dhu_dhd;

dy_dhd = sparse（ [ dyprima_dhd zeros（51）;
            zeros（12）      1 ] ）;

Ri = speye（2）*std_pxl^2;

Padd = sparse（ [Ri  zeros（21）;
    zeros（12）  std_rho^2] ）;

%
P_xv = P（ 1:13 1:13 ）;
P_yxv = P（ 14:end 1:13 ）;
P_y = P（ 14:end 14:end ）;
P_xvy = P（ 1:13 14:end ）;
P_RES = [ P_xv          P_xvy                       P_xv*dy_dxv‘;
          P_yxv         P_y                         P_yxv*dy_dxv‘;
          dy_dxv*P_xv   dy_dxv*P_xvy                dy_dxv*P_xv*dy_dxv‘+...
          

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

    I.A....         3  2017-12-14 19:39  ekfmonoslam\.svn\entries

    I.A....         3  2017-12-14 19:39  ekfmonoslam\.svn\format

    I.A...R      1327  2017-12-14 19:39  ekfmonoslam\.svn\pristine\03\033a001575e80c9f7d8753be8e90234e2232c227.svn-base

    I.A...R      3326  2017-12-14 20:06  ekfmonoslam\.svn\pristine\07\0758be254ed20cd9ab8420be9282c71200ec4ad5.svn-base

    I.A...R      2018  2017-12-14 19:39  ekfmonoslam\.svn\pristine\08\08e4e160e3291e6a1a296b6f89f59b5ac90f5719.svn-base

    I.A...R      1313  2017-12-14 19:39  ekfmonoslam\.svn\pristine\0c\0c51429c746aefa0f8e00c45039d4295447b12ec.svn-base

    I.A...R      1789  2017-12-14 20:06  ekfmonoslam\.svn\pristine\0d\0d3e40a4a3d82ad8230381d8b377d453710f9696.svn-base

    I.A...R      2479  2017-12-14 19:39  ekfmonoslam\.svn\pristine\0e\0ea798aeba666f4beec77bb6e642f79617c6bc99.svn-base

    I.A...R      1597  2017-12-14 19:39  ekfmonoslam\.svn\pristine\0f\0f5e54bd09d582db3177d2f02dfbd6637de0b2a3.svn-base

    I.A...R      3813  2017-12-14 20:06  ekfmonoslam\.svn\pristine\11\11539ed46bfed5865732d1979af03cec42b3f8a4.svn-base

    I.A...R      1973  2017-12-14 19:39  ekfmonoslam\.svn\pristine\14\14c6038c6b968f87fa86564205568189859ab0cd.svn-base

    I.A...R       321  2017-12-14 20:06  ekfmonoslam\.svn\pristine\16\16b6f6fc3a5c574fbadb0db53f0b6d611dbb8cb9.svn-base

    I.A...R      1438  2017-12-14 19:39  ekfmonoslam\.svn\pristine\19\1987ff338ecf6649d236478a94dc62b08a0af6ba.svn-base

    I.A...R      5126  2017-12-14 19:39  ekfmonoslam\.svn\pristine\1a\1a869286e39159cbcfa305ccbd95e9c30358f959.svn-base

    I.A...R      1865  2017-12-14 19:39  ekfmonoslam\.svn\pristine\20\20de4a03acc85fa43555b9288d22357d09a2c32f.svn-base

    I.A...R      1532  2017-12-14 20:06  ekfmonoslam\.svn\pristine\27\27d6a50fb923afd66fb6dcf2073cc2be4ba6e94f.svn-base

    I.A...R      1753  2017-12-14 19:39  ekfmonoslam\.svn\pristine\29\2980d2c708d1517503794c0eacb6984041fbcda7.svn-base

    I.A...R      3905  2017-12-14 20:06  ekfmonoslam\.svn\pristine\2b\2bdee5218c620cc6be2de94318074c180d81a0ad.svn-base

    I.A...R      1639  2017-12-14 19:39  ekfmonoslam\.svn\pristine\32\3235b465b4331b719065237601ad38e9bddbc3f5.svn-base

    I.A...R      1335  2017-12-14 19:39  ekfmonoslam\.svn\pristine\33\33cb887fee49019214abb8ab1b602538f5ee1c1e.svn-base

    I.A...R      2459  2017-12-14 19:39  ekfmonoslam\.svn\pristine\34\34a098936fff6aa0d2ba1a257f3470d546d034f1.svn-base

    I.A...R      1786  2017-12-14 19:39  ekfmonoslam\.svn\pristine\34\34d16e28bef6863bf9bc6a185df87dcfa1aa6377.svn-base

    I.A...R      1743  2017-12-14 19:39  ekfmonoslam\.svn\pristine\35\351ab106706cd5e01bf0fdc691419eba9fd7f3b5.svn-base

    I.A...R    221316  2017-12-14 20:06  ekfmonoslam\.svn\pristine\38\3812a64443c2b6ef4ec2839b6070ed6fb20ff7cf.svn-base

    I.A...R    164469  2017-12-14 20:06  ekfmonoslam\.svn\pristine\39\3979604a19f967e5b9b9fc52c51f7cf36709196a.svn-base

    I.A...R      1213  2017-12-14 20:06  ekfmonoslam\.svn\pristine\3b\3b1ac77aa4b4920f96c1b3d055b1d391a87627f7.svn-base

    I.A...R      3532  2017-12-14 20:06  ekfmonoslam\.svn\pristine\3b\3b583517006a128faf31e26660bbba7e8465c8d9.svn-base

    I.A...R      1545  2017-12-14 19:39  ekfmonoslam\.svn\pristine\3e\3e7565585f97ec9c099df5321ebc690bfd1259d5.svn-base

    I.A...R      2185  2017-12-14 20:06  ekfmonoslam\.svn\pristine\3f\3f9b1db753476d5dbe547a56ba8301921a8e495a.svn-base

    I.A...R      1473  2017-12-14 20:06  ekfmonoslam\.svn\pristine\44\4496495c528e339e19d499228dfbc17493aa9aa1.svn-base

............此处省略347个文件信息