💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文目录如下：🎁🎁🎁
目录
⛳️赠与读者
👨💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。
或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎
💥1 概述
文献来源：
摘要
本文从可观测性的角度研究了基于扩展卡尔曼滤波器（EKF）的同时定位与地图构建（SLAM）中的不一致性问题。我们通过分析证明，当过程和测量模型的雅可比矩阵在每个时间步骤中根据最新的状态估计值进行评估时，EKF中使用的线性化误差状态系统具有比实际非线性SLAM系统更高维度的可观测子空间。因此，EKF的协方差估计在状态空间中无信息可用的方向上会减小，这是不一致性的主要原因之一。基于这些理论结果，我们提出了一个改进EKF SLAM一致性的通用框架。在这个框架中，EKF的线性化点被选择以确保得到的线性化系统模型具有适当维度的可观测子空间。我们描述了两种算法，它们是这一范式的实例。第一种算法称为可观测性约束（OC）-EKF，其选择线性化点以最小化它们的预期误差（即线性化点与真实状态之间的差异），并满足可观测性约束。第二种算法中，滤波器的雅可比矩阵使用所有状态变量的第一次可用估计进行计算。这种方法称为第一估计雅可比矩阵（FEJ）-EKF。提出的算法经过了仿真和实验测试，显示出在准确性和一致性方面明显优于标准EKF。
关键词：同时定位与地图构建，非线性估计，扩展卡尔曼滤波器，线性化误差，估计器不一致性，可观测性
📚2 运行结果
部分代码：
sigma_v = sigma/sqrt(2); %.1*v_true; %
sigma_w = 2*sqrt(2)*sigma; %.1*omega_true; 1*pi/180; %
Q = diag([sigma_v^2 sigma_w^2]);
sigma_p = .1; %noise is the percentagae of distance measurement, BUT double check rws.m since sometimes we use this as a constant absolute sigma
sigma_r = 1; %range measmnt noise
sigma_th = 10*pi/180;%bearing measuremnt noise
nL = 20; %number of landmarks
nSteps = 2500; %nubmer of time steps
nRuns = 5; %number of monte carlo runs
if nL==1
max_range = 200;%always observe this landmark
else
max_range = 5;%env_size/10;%.5*v_true/omega_true;%
end
min_range = .5;
init_steps = 0;%3
max_delay = 10;%for delayed initial
NIncr = 0; %increment number of incremental MAP: 0 - not runing
%% preallocate memory for saving resutls
% Ideal EKF
xRest_id = zeros(3,nSteps,nRuns); %estimated traj
xRerr_id = zeros(3,nSteps,nRuns); %all err state
Prr_id = zeros(3,nSteps,nRuns); %actually diag of Prr
neesR_id = zeros(1,nSteps,nRuns); %nees (or mahalanobis distance)
rmsRp_id = zeros(1,nSteps,nRuns); %rms of robot position
rmsRth_id = zeros(1,nSteps,nRuns); %rms of robot orientation
xLest_id = zeros(2,nL,nSteps,nRuns);
xLerr_id = zeros(2,nL,nSteps,nRuns);
Pll_id = zeros(2,nL,nSteps,nRuns);
neesL_id = zeros(1,nL,nSteps,nRuns);
rmsL_id = zeros(1,nL,nSteps,nRuns);
nees_id = zeros(1,nSteps,nRuns); %nees for the whole state
% Standard EKF
xRest_std = zeros(3,nSteps,nRuns); %estimated traj
xRerr_std = zeros(3,nSteps,nRuns); %all err state
Prr_std = zeros(3,nSteps,nRuns); %actually diag of Prr
neesR_std = zeros(1,nSteps,nRuns); %nees (or mahalanobis distance)
rmsRp_std = zeros(1,nSteps,nRuns); %rms of robot position
rmsRth_std = zeros(1,nSteps,nRuns); %rms of robot orientation
xLest_std = zeros(2,nL,nSteps,nRuns);
xLerr_std = zeros(2,nL,nSteps,nRuns);
Pll_std = zeros(2,nL,nSteps,nRuns);
neesL_std = zeros(1,nL,nSteps,nRuns);
rmsL_std = zeros(1,nL,nSteps,nRuns);
nees_std = zeros(1,nSteps,nRuns); %nees for the whole state
kld_std = zeros(1,nSteps,nRuns); % KLD
% FEJ-EKF
xRest_fej = zeros(3,nSteps,nRuns); %estimated traj
xRerr_fej = zeros(3,nSteps,nRuns); %all err state
Prr_fej = zeros(3,nSteps,nRuns); %actually diag of Prr
neesR_fej = zeros(1,nSteps,nRuns); %nees (or mahalanobis distance)
rmsRp_fej = zeros(1,nSteps,nRuns); %rms of robot position
rmsRth_fej = zeros(1,nSteps,nRuns); %rms of robot orientation
xLest_fej = zeros(2,nL,nSteps,nRuns);
xLerr_fej = zeros(2,nL,nSteps,nRuns);
Pll_fej = zeros(2,nL,nSteps,nRuns);
neesL_fej = zeros(1,nL,nSteps,nRuns);
rmsL_fej = zeros(1,nL,nSteps,nRuns);
nees_fej = zeros(1,nSteps,nRuns); %nees for the whole state
kld_fej = zeros(1,nSteps,nRuns); % KLD
% OC-EKF
xRest_ocekf_1 = zeros(3,nSteps,nRuns); %estimated traj
xRerr_ocekf_1 = zeros(3,nSteps,nRuns); %all err state
Prr_ocekf_1 = zeros(3,nSteps,nRuns); %actually diag of Prr
neesR_ocekf_1 = zeros(1,nSteps,nRuns); %nees (or mahalanobis distance)
rmsRp_ocekf_1 = zeros(1,nSteps,nRuns); %rms of robot position
rmsRth_ocekf_1 = zeros(1,nSteps,nRuns); %rms of robot orientation
xLest_ocekf_1 = zeros(2,nL,nSteps,nRuns);
xLerr_ocekf_1 = zeros(2,nL,nSteps,nRuns);
Pll_ocekf_1 = zeros(2,nL,nSteps,nRuns);
neesL_ocekf_1 = zeros(1,nL,nSteps,nRuns);
rmsL_ocekf_1 = zeros(1,nL,nSteps,nRuns);
nees_ocekf_1 = zeros(1,nSteps,nRuns); %nees for the whole state
kld_ocekf_1 = zeros(1,nSteps,nRuns); % KLD
%% LANDMARK GENERATION: same landmarks in each run
if nL==1
xL_true_fixed = [0;v_true/omega_true];
elseif nL==2
xL_true_fixed = [5 -5; 5 15];
else
xL_true_fixed = gen_map(nL,v_true,omega_true,min_range, max_range, nSteps,dt);%max_range=5
end
%% Monte Carlo Simulations
tic
for kk = 1:nRuns
kk
% % real world simulation data % %
xL_true(:,:,kk) = xL_true_fixed;
[v_m,omega_m, v_true_all,omega_true_all, xR_true(:,:,kk), z,R] = rws(nSteps, dt,v_true,omega_true,sigma_v,sigma_w,sigma_r,sigma_th,sigma_p,xL_true(:,:,kk),max_range,min_range);
% % INITIALIZATION
x0 = zeros(3,1);
if init_steps
P0 = zeros(3);
else
P0 = diag([(.0001)^2,(.0001)^2,(.0001)^2]);
end
for k=1:init_steps
[x0,P0] = propagate_std(x0,P0,dt,v_m(k),omega_m(k),sigma_v,sigma_w);
end
% Ideal EKF
xe_id = x0;
Pe_id = P0;
V_id = [];
% Standard EKF
xe_std = x0;
Pe_std = P0;
V_std = [];
% FEJ-EKF
xe_fej = x0;
Pe_fej = P0;
xL_fej_1 = [];
xR_fej_k_k1 = xe_fej(1:3,1);
PHI_mult_fej = eye(1); %propagation jacobian product
V_fej = [];
% OC-EKF
xe_ocekf_1 = x0;
Pe_ocekf_1 = P0;
xL_ocekf_1_1 = [];
xR_oc_k_k1_1 = xe_ocekf_1(1:3,1);
dpR_star_prev_1 = zeros(2,1);
pR_star_prev = x0(1:2,1);
dpR_ocekf_1 = zeros(2,1);
V_ocekf_1 = [];
PHI_mult_ocekf_1 = eye(1);
lambda_1 = zeros(2,nL);
% list of landmark ids that sequentially appear in the state vector
lm_seq_id = [];
lm_seq_std = [];
lm_seq_fej = [];
lm_seq_ocekf_1 = [];
🎉3 参考文献
文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。
🌈4 Matlab代码、文章
资料获取，更多粉丝福利，MATLAB|Simulink|Python资源获取
7908
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
