一、EKF-SLAM算法原理
1.1 基本概念
EKF-SLAM（扩展卡尔曼滤波-同时定位与建图）是一种基于概率的算法，用于机器人在未知环境中同时估计自身位姿和构建环境地图。
1.2 状态向量表示
状态向量 x = [机器人位姿; 地标位置]
x = [x_r, y_r, θ_r, x_l1, y_l1, x_l2, y_l2, ...]^T1.3 算法流程
- 预测步骤：根据运动模型预测机器人位姿
- 更新步骤：根据观测数据更新状态估计
- 数据关联：将观测与已有地标匹配
- 新地标初始化：将未匹配的观测初始化为新地标
二、MATLAB仿真实现
2.1 完整仿真代码
matlab
%% EKF-SLAM MATLAB仿真
% 功能：实现基本的EKF-SLAM算法仿真
clear all; close all; clc;
%% 1. 仿真参数设置
fprintf('=== EKF-SLAM仿真开始 ===\n');
% 时间参数
dt = 0.1; % 时间步长(s)
T = 100; % 总仿真时间(s)
steps = T/dt; % 总步数
% 机器人参数
wheel_base = 0.5; % 轮距(m)
wheel_radius = 0.1; % 轮半径(m)
% 噪声参数
Q = diag([0.1, 0.1, 0.05].^2); % 过程噪声协方差
R = diag([0.1, 0.05].^2); % 观测噪声协方差
% 地图参数
num_landmarks = 10; % 地标数量
map_size = 20; % 地图尺寸(m)
% EKF参数
state_dim = 3; % 机器人状态维度
landmark_dim = 2; % 每个地标维度
%% 2. 环境初始化
fprintf('初始化环境...\n');
% 生成随机地标
true_landmarks = map_size * (rand(2, num_landmarks) - 0.5);
% 机器人真实轨迹（圆形运动）
true_trajectory = zeros(3, steps);
estimated_trajectory = zeros(3, steps);
% 初始状态
true_pose = [0; 0; 0]; % [x, y, theta]
estimated_pose = true_pose + sqrt(Q)*randn(3,1); % 加入初始误差
% EKF状态向量和协方差初始化
state = [estimated_pose; zeros(2*num_landmarks, 1)]; % 初始时地标位置未知
P = zeros(3 + 2*num_landmarks);
P(1:3, 1:3) = 0.1 * eye(3); % 机器人位姿初始不确定性
for i = 1:num_landmarks
idx = 3 + 2*(i-1) + (1:2);
P(idx, idx) = 1000 * eye(2); % 地标位置初始不确定性（大值表示未知）
end
% 数据关联矩阵（记录哪些地标已被观测）
observed_landmarks = false(1, num_landmarks);
%% 3. 主仿真循环
fprintf('开始仿真循环...\n');
% 创建图形窗口
figure('Position', [100, 100, 1200, 500]);
for k = 1:steps
%% 3.1 生成控制输入（速度模型）
v = 1.0 + 0.1*randn; % 线速度(m/s)
w = 0.2 + 0.05*randn; % 角速度(rad/s)
%% 3.2 真实运动（用于生成真值）
true_pose = motion_model(true_pose, [v; w], dt, zeros(3,1));
true_trajectory(:, k) = true_pose;
%% 3.3 EKF预测步骤
% 提取机器人状态
robot_state = state(1:3);
% 计算雅可比矩阵
F_x = [1, 0, -v*dt*sin(robot_state(3));
0, 1, v*dt*cos(robot_state(3));
0, 0, 1];
% 状态转移矩阵
F = eye(length(state));
F(1:3, 1:3) = F_x;
% 过程噪声雅可比
G = [dt*cos(robot_state(3)), 0;
dt*sin(robot_state(3)), 0;
0, dt];
% 预测状态
state(1:3) = motion_model(robot_state, [v; w], dt, zeros(3,1));
% 预测协方差
P = F * P * F' + Q_k;
%% 3.4 生成观测
measurements = [];
landmark_ids = [];
% 传感器参数
max_range = 10; % 最大观测距离(m)
fov = 120; % 视场角(度)
for i = 1:num_landmarks
% 计算相对位置
dx = true_landmarks(1, i) - true_pose(1);
dy = true_landmarks(2, i) - true_pose(2);
% 计算距离和角度
distance = sqrt(dx^2 + dy^2);
bearing = atan2(dy, dx) - true_pose(3);
bearing = wrapToPi(bearing); % 归一化到[-pi, pi]
% 检查是否在传感器范围内
if distance <= max_range && abs(bearing) <= deg2rad(fov/2)
% 添加观测噪声
z_noisy = [distance; bearing] + sqrt(R)*randn(2,1);
measurements = [measurements, z_noisy];
landmark_ids = [landmark_ids, i];
end
end
%% 3.5 EKF更新步骤
if ~isempty(measurements)
num_measurements = size(measurements, 2);
for m = 1:num_measurements
landmark_id = landmark_ids(m);
z = measurements(:, m);
% 检查地标是否已初始化
if observed_landmarks(landmark_id)
% 已观测地标：数据关联
idx = 3 + 2*(landmark_id-1) + (1:2);
landmark_state = state(idx);
% 计算预测观测
dx = landmark_state(1) - state(1);
dy = landmark_state(2) - state(2);
q = dx^2 + dy^2;
z_pred = [sqrt(q);
atan2(dy, dx) - state(3)];
z_pred(2) = wrapToPi(z_pred(2));
% 计算观测雅可比
H = zeros(2, length(state));
H(:, 1:3) = [-dx/sqrt(q), -dy/sqrt(q), 0;
dy/q, -dx/q, -1];
H(:, idx) = [dx/sqrt(q), dy/sqrt(q);
-dy/q, dx/q];
% 卡尔曼增益
S = H * P * H' + R;
K = P * H' / S;
% 状态更新
innovation = z - z_pred;
innovation(2) = wrapToPi(innovation(2)); % 角度归一化
state = state + K * innovation;
P = (eye(length(state)) - K * H) * P;
else
% 新地标：初始化
fprintf('初始化新地标 %d\n', landmark_id);
% 计算地标全局坐标
global_x = state(1) + z(1)*cos(state(3) + z(2));
global_y = state(2) + z(1)*sin(state(3) + z(2));
% 更新状态向量
idx = 3 + 2*(landmark_id-1) + (1:2);
state(idx) = [global_x; global_y];
% 更新协方差（简化处理）
observed_landmarks(landmark_id) = true;
end
end
end
%% 3.6 存储估计轨迹
estimated_trajectory(:, k) = state(1:3);
%% 3.7 实时可视化
if mod(k, 10) == 0 || k == steps
% 子图1：轨迹和地图
subplot(1, 2, 1);
cla;
hold on;
grid on;
% 绘制真实地标
plot(true_landmarks(1, :), true_landmarks(2, :), 'g^', ...
'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '真实地标');
% 绘制估计地标
for i = 1:num_landmarks
if observed_landmarks(i)
idx = 3 + 2*(i-1) + (1:2);
plot(state(idx(1)), state(idx(2)), 'b*', ...
'MarkerSize', 8, 'LineWidth', 1.5);
% 绘制不确定性椭圆
covariance = P(idx, idx);
error_ellipse(covariance, state(idx), 'b', 0.5);
end
end
% 绘制真实轨迹
plot(true_trajectory(1, 1:k), true_trajectory(2, 1:k), 'r-', ...
'LineWidth', 1.5, 'DisplayName', '真实轨迹');
% 绘制估计轨迹
plot(estimated_trajectory(1, 1:k), estimated_trajectory(2, 1:k), 'b--', ...
'LineWidth', 1.5, 'DisplayName', '估计轨迹');
% 绘制当前位姿
plot_robot(true_pose, 'r', 0.5); % 真实位姿
plot_robot(state(1:3), 'b', 0.5); % 估计位姿
% 绘制观测
if ~isempty(measurements)
for m = 1:size(measurements, 2)
z = measurements(:, m);
end_x = true_pose(1) + z(1)*cos(true_pose(3) + z(2));
end_y = true_pose(2) + z(1)*sin(true_pose(3) + z(2));
plot([true_pose(1), end_x], [true_pose(2), end_y], 'm:', 'LineWidth', 1);
end
end
xlabel('X (m)'); ylabel('Y (m)');
title(sprintf('EKF-SLAM仿真 (t=%.1fs, 步数=%d)', k*dt, k));
legend('Location', 'best');
axis equal;
xlim([-map_size/2, map_size/2]);
ylim([-map_size/2, map_size/2]);
% 子图2：误差分析
subplot(1, 2, 2);
cla;
hold on;
grid on;
% 计算位置误差
pos_error = sqrt((true_pose(1) - state(1))^2 + ...
(true_pose(2) - state(2))^2);
% 计算角度误差
angle_error = abs(wrapToPi(true_pose(3) - state(3)));
% 绘制误差历史
if k > 1
time_history = (1:k)*dt;
pos_errors = sqrt(sum((true_trajectory(1:2, 1:k) - ...
estimated_trajectory(1:2, 1:k)).^2, 1));
plot(time_history, pos_errors, 'b-', 'LineWidth', 1.5);
plot(k*dt, pos_error, 'ro', 'MarkerSize', 8, 'LineWidth', 2);
end
xlabel('时间 (s)'); ylabel('位置误差 (m)');
title(sprintf('位置误差: %.3fm, 角度误差: %.3frad', pos_error, angle_error));
legend('误差历史', '当前误差', 'Location', 'best');
drawnow;
end
%% 3.8 进度显示
if mod(k, 100) == 0
fprintf('进度: %.1f%%\n', k/steps*100);
end
end
%% 4. 结果分析
fprintf('\n=== 仿真完成 ===\n');
% 计算最终误差
final_pos_error = norm(true_trajectory(1:2, end) - estimated_trajectory(1:2, end));
final_angle_error = abs(wrapToPi(true_trajectory(3, end) - estimated_trajectory(3, end)));
fprintf('最终位置误差: %.3f m\n', final_pos_error);
fprintf('最终角度误差: %.3f rad\n', final_angle_error);
fprintf('已观测地标数: %d/%d\n', sum(observed_landmarks), num_landmarks);
% 绘制最终结果对比图
figure('Position', [100, 100, 800, 600]);
% 子图1：完整轨迹对比
subplot(2, 2, 1);
hold on; grid on;
plot(true_trajectory(1, :), true_trajectory(2, :), 'r-', 'LineWidth', 2, 'DisplayName', '真实轨迹');
plot(estimated_trajectory(1, :), estimated_trajectory(2, :), 'b--', 'LineWidth', 2, 'DisplayName', '估计轨迹');
plot(true_landmarks(1, :), true_landmarks(2, :), 'g^', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '真实地标');
% 绘制估计地标
for i = 1:num_landmarks
if observed_landmarks(i)
idx = 3 + 2*(i-1) + (1:2);
plot(state(idx(1)), state(idx(2)), 'b*', 'MarkerSize', 8, 'LineWidth', 1.5);
end
end
xlabel('X (m)'); ylabel('Y (m)');
title('轨迹与地图对比');
legend('Location', 'best');
axis equal;
% 子图2：位置误差随时间变化
subplot(2, 2, 2);
time = (1:steps)*dt;
pos_errors = sqrt(sum((true_trajectory(1:2, :) - estimated_trajectory(1:2, :)).^2, 1));
plot(time, pos_errors, 'b-', 'LineWidth', 1.5);
grid on;
xlabel('时间 (s)'); ylabel('位置误差 (m)');
title('位置误差变化');
ylim([0, max(pos_errors)*1.1]);
% 子图3：角度误差随时间变化
subplot(2, 2, 3);
angle_errors = abs(wrapToPi(true_trajectory(3, :) - estimated_trajectory(3, :)));
plot(time, angle_errors, 'r-', 'LineWidth', 1.5);
grid on;
xlabel('时间 (s)'); ylabel('角度误差 (rad)');
title('角度误差变化');
ylim([0, max(angle_errors)*1.1]);
% 子图4：协方差迹（不确定性度量）
subplot(2, 2, 4);
cov_trace = zeros(1, steps);
for k = 1:steps
% 这里简化处理，实际应该存储每一步的协方差迹
cov_trace(k) = trace(P(1:3, 1:3));
end
plot(time, cov_trace, 'g-', 'LineWidth', 1.5);
grid on;
xlabel('时间 (s)'); ylabel('协方差迹');
title('机器人位姿不确定性');
%% 辅助函数定义
function new_pose = motion_model(pose, u, dt, noise)
% 运动模型：速度模型
% pose: [x, y, theta]
% u: [v, w] 线速度和角速度
% dt: 时间步长
% noise: 过程噪声
v = u(1);
w = u(2);
theta = pose(3);
if abs(w) < 1e-6
% 直线运动
new_pose = pose + [v*dt*cos(theta);
v*dt*sin(theta);
0] + noise;
else
% 曲线运动
new_pose = pose + [v/w*(sin(theta + w*dt) - sin(theta));
v/w*(-cos(theta + w*dt) + cos(theta));
w*dt] + noise;
end
% 角度归一化
new_pose(3) = wrapToPi(new_pose(3));
end
function plot_robot(pose, color, scale)
% 绘制机器人
% pose: [x, y, theta]
% color: 颜色
% scale: 缩放因子
x = pose(1);
y = pose(2);
theta = pose(3);
% 机器人尺寸
robot_length = 0.6 * scale;
robot_width = 0.4 * scale;
% 机器人轮廓（矩形）
corners = [-robot_length/2, -robot_width/2;
robot_length/2, -robot_width/2;
robot_length/2, robot_width/2;
-robot_length/2, robot_width/2];
% 旋转矩阵
R = [cos(theta), -sin(theta);
sin(theta), cos(theta)];
% 旋转和平移
corners = (R * corners')' + [x, y];
% 绘制机器人主体
fill(corners(:,1), corners(:,2), color, 'FaceAlpha', 0.3, 'EdgeColor', color, 'LineWidth', 1.5);
% 绘制方向指示器
front_x = x + robot_length/2 * cos(theta);
front_y = y + robot_length/2 * sin(theta);
plot([x, front_x], [y, front_y], color, 'LineWidth', 2);
% 绘制机器人中心点
plot(x, y, [color, 'o'], 'MarkerSize', 6, 'LineWidth', 2);
end
function h = error_ellipse(covariance, center, color, scale)
% 绘制误差椭圆
% covariance: 2x2协方差矩阵
% center: 椭圆中心 [x, y]
% color: 颜色
% scale: 缩放因子（置信度）
[V, D] = eig(covariance);
% 计算椭圆参数
a = scale * sqrt(D(1,1)); % 半长轴
b = scale * sqrt(D(2,2)); % 半短轴
angle = atan2(V(2,1), V(1,1)); % 旋转角度
% 生成椭圆点
theta = linspace(0, 2*pi, 100);
ellipse = [a * cos(theta); b * sin(theta)];
% 旋转
R = [cos(angle), -sin(angle); sin(angle), cos(angle)];
ellipse = R * ellipse;
% 平移
ellipse(1,:) = ellipse(1,:) + center(1);
ellipse(2,:) = ellipse(2,:) + center(2);
% 绘制椭圆
h = plot(ellipse(1,:), ellipse(2,:), color, 'LineWidth', 1);
end
function angle = wrapToPi(angle)
% 将角度归一化到[-pi, pi]
angle = mod(angle + pi, 2*pi) - pi;
end2.2 简化版本（快速入门）
matlab
%% 简化版EKF-SLAM仿真
% 适用于快速理解和测试
clear; close all; clc;
% 参数设置
dt = 0.1;
steps = 200;
num_landmarks = 5;
% 初始化
true_landmarks = 10 * rand(2, num_landmarks) - 5;
true_pose = [0; 0; 0];
state = [true_pose; zeros(2*num_landmarks, 1)];
P = diag([0.1, 0.1, 0.05, 1000*ones(1, 2*num_landmarks)]);
% 噪声
Q = diag([0.05, 0.05, 0.02].^2);
R = diag([0.1, 0.05].^2);
% 存储
trajectory = zeros(3, steps);
estimates = zeros(3, steps);
% 主循环
for k = 1:steps
% 控制输入
v = 1.0;
w = 0.1 * sin(0.1*k);
% 真实运动
true_pose = true_pose + [v*dt*cos(true_pose(3));
v*dt*sin(true_pose(3));
w*dt];
trajectory(:, k) = true_pose;
% EKF预测
state(1:3) = state(1:3) + [v*dt*cos(state(3));
v*dt*sin(state(3));
w*dt];
% 雅可比
F = eye(length(state));
F(1:3, 1:3) = [1, 0, -v*dt*sin(state(3));
0, 1, v*dt*cos(state(3));
0, 0, 1];
% 预测协方差
P = F * P * F' + blkdiag(Q, zeros(2*num_landmarks));
% 生成观测
for i = 1:num_landmarks
dx = true_landmarks(1,i) - true_pose(1);
dy = true_landmarks(2,i) - true_pose(2);
dist = norm([dx, dy]);
if dist < 5 % 观测范围
% 观测模型
z_true = [dist;
atan2(dy, dx) - true_pose(3)];
z = z_true + sqrt(R)*randn(2,1);
% 数据关联（简化：已知对应关系）
idx = 3 + 2*(i-1) + (1:2);
if norm(state(idx)) < 1e-6 % 新地标
state(idx) = [state(1) + z(1)*cos(state(3)+z(2));
state(2) + z(1)*sin(state(3)+z(2))];
else % 已存在地标
% 预测观测
dx_est = state(idx(1)) - state(1);
dy_est = state(idx(2)) - state(2);
q = dx_est^2 + dy_est^2;
z_pred = [sqrt(q);
atan2(dy_est, dx_est) - state(3)];
% 雅可比
H = zeros(2, length(state));
H(:, 1:3) = [-dx_est/sqrt(q), -dy_est/sqrt(q), 0;
dy_est/q, -dx_est/q, -1];
H(:, idx) = [dx_est/sqrt(q), dy_est/sqrt(q);
-dy_est/q, dx_est/q];
% 卡尔曼更新
S = H * P * H' + R;
K = P * H' / S;
state = state + K * (z - z_pred);
P = (eye(length(state)) - K * H) * P;
end
end
end
estimates(:, k) = state(1:3);
% 可视化
if mod(k, 10) == 0
clf;
subplot(1,2,1);
hold on; grid on;
plot(trajectory(1,1:k), trajectory(2,1:k), 'r-');
plot(estimates(1,1:k), estimates(2,1:k), 'b--');
plot(true_landmarks(1,:), true_landmarks(2,:), 'g^');
for i = 1:num_landmarks
idx = 3 + 2*(i-1) + (1:2);
if norm(state(idx)) > 0
plot(state(idx(1)), state(idx(2)), 'b*');
end
end
xlabel('X'); ylabel('Y'); title('EKF-SLAM');
legend('真实轨迹', '估计轨迹', '真实地标', '估计地标');
axis equal;
subplot(1,2,2);
error = vecnorm(trajectory(1:2,1:k) - estimates(1:2,1:k));
plot(1:k, error, 'b-');
xlabel('步数'); ylabel('位置误差'); title('误差变化');
grid on;
drawnow;
end
end
fprintf('仿真完成！\n');三、MATLAB工具箱实现
3.1 使用Robotics System Toolbox
matlab
%% 使用MATLAB Robotics System Toolbox实现EKF-SLAM
% 创建EKF-SLAM对象
ekfSLAM = robotics.EKFSLAM('StateTransitionFcn', @stateTransitionFcn, ...
'MeasurementFcn', @measurementFcn, ...
'StateCovariance', eye(3), ...
'ProcessNoise', diag([0.1, 0.1, 0.05]), ...
'MeasurementNoise', diag([0.1, 0.05]));
% 初始化状态
initialState = [0; 0; 0]; % [x, y, theta]
initialize(ekfSLAM, initialState);
% 仿真循环
for k = 1:100
% 控制输入
u = [1.0; 0.1]; % [v; w]
% 预测步骤
predict(ekfSLAM, u);
% 生成观测
measurements = generateMeasurements(truePose, trueLandmarks);
% 更新步骤
if ~isempty(measurements)
correct(ekfSLAM, measurements);
end
% 获取估计状态
[estimatedState, stateCovariance] = getState(ekfSLAM);
% 可视化
plotSLAM(ekfSLAM, truePose, trueLandmarks);
end3.2 自定义函数
matlab
function statePred = stateTransitionFcn(state, u)
% 状态转移函数
dt = 0.1;
v = u(1);
w = u(2);
theta = state(3);
if abs(w) < 1e-6
statePred = state + [v*dt*cos(theta);
v*dt*sin(theta);
0];
else
statePred = state + [v/w*(sin(theta+w*dt)-sin(theta));
v/w*(-cos(theta+w*dt)+cos(theta));
w*dt];
end
statePred(3) = wrapToPi(statePred(3));
end
function measurements = measurementFcn(state, landmarks)
% 观测函数
measurements = [];
for i = 1:size(landmarks, 2)
dx = landmarks(1,i) - state(1);
dy = landmarks(2,i) - state(2);
dist = norm([dx, dy]);
bearing = atan2(dy, dx) - state(3);
measurements = [measurements; dist; wrapToPi(bearing)];
end
end四、性能优化技巧
4.1 稀疏矩阵优化
matlab
% 利用EKF-SLAM的稀疏性
function P = updateSparseCovariance(P, H, R, z, z_pred)
% 稀疏更新协方差矩阵
n = size(P, 1);
m = size(z, 1);
% 只更新相关部分
idx = find(any(H ~= 0, 1)); % 找到非零列
H_reduced = H(:, idx);
P_reduced = P(idx, idx);
% 计算卡尔曼增益（小矩阵运算）
S = H_reduced * P_reduced * H_reduced' + R;
K_reduced = P_reduced * H_reduced' / S;
% 更新状态和协方差
innovation = z - z_pred;
P(idx, idx) = P_reduced - K_reduced * H_reduced * P_reduced;
% 返回更新后的协方差
end4.2 并行化处理
matlab
% 使用parfor并行处理观测
if ~isempty(measurements)
num_meas = size(measurements, 2);
innovations = zeros(2, num_meas);
H_cells = cell(1, num_meas);
parfor m = 1:num_meas
% 并行计算每个观测的创新向量和雅可比
[innovations(:, m), H_cells{m}] = ...
computeInnovation(state, P, measurements(:, m), R);
end
% 合并更新
for m = 1:num_meas
H = H_cells{m};
S = H * P * H' + R;
K = P * H' / S;
state = state + K * innovations(:, m);
P = (eye(size(P)) - K * H) * P;
end
end五、扩展功能
5.1 数据关联（最近邻算法）
matlab
function [matched_idx, new_landmarks] = dataAssociation(state, measurements, max_distance)
% 数据关联：最近邻算法
num_landmarks = (length(state) - 3) / 2;
matched_idx = zeros(1, size(measurements, 2));
new_landmarks = [];
for m = 1:size(measurements, 2)
z = measurements(:, m);
min_dist = inf;
best_idx = 0;
% 计算与所有已存在地标的马氏距离
for i = 1:num_landmarks
idx = 3 + 2*(i-1) + (1:2);
if norm(state(idx)) > 0 % 地标已初始化
% 计算预测观测
dx = state(idx(1)) - state(1);
dy = state(idx(2)) - state(2);
z_pred = [norm([dx, dy]);
atan2(dy, dx) - state(3)];
% 计算马氏距离
H = computeJacobian(state, i);
S = H * P * H' + R;
d = (z - z_pred)' / S * (z - z_pred);
if d < min_dist && d < max_distance
min_dist = d;
best_idx = i;
end
end
end
if best_idx > 0
matched_idx(m) = best_idx;
else
% 新地标
new_landmarks = [new_landmarks, m];
end
end
end5.2 一致性检验（NEES测试）
matlab
function [passed, nees] = consistencyTest(true_state, est_state, est_cov)
% NEES（归一化估计误差平方）测试
error = true_state(1:3) - est_state(1:3);
nees = error' / est_cov(1:3, 1:3) * error;
% 95%置信区间（卡方分布，自由度为3）
chi2_lower = chi2inv(0.025, 3);
chi2_upper = chi2inv(0.975, 3);
passed = (nees >= chi2_lower) && (nees <= chi2_upper);
if ~passed
fprintf('NEES测试失败: %.3f (应在[%.3f, %.3f]之间)\n', ...
nees, chi2_lower, chi2_upper);
end
end参考代码 EKF-SLAM在matlab上实现仿真 www.youwefan.com/contentcsu/59426.html
六、仿真结果分析
6.1 性能指标计算
matlab
% 计算RMSE
function [rmse_position, rmse_orientation] = computeRMSE(true_traj, est_traj)
position_errors = sqrt(sum((true_traj(1:2,:) - est_traj(1:2,:)).^2, 1));
orientation_errors = abs(wrapToPi(true_traj(3,:) - est_traj(3,:)));
rmse_position = sqrt(mean(position_errors.^2));
rmse_orientation = sqrt(mean(orientation_errors.^2));
fprintf('位置RMSE: %.4f m\n', rmse_position);
fprintf('角度RMSE: %.4f rad\n', rmse_orientation);
end
% 计算地图精度
function map_accuracy = computeMapAccuracy(true_landmarks, est_landmarks)
% 使用匈牙利算法进行地标匹配
num_true = size(true_landmarks, 2);
num_est = size(est_landmarks, 2);
% 计算距离矩阵
dist_matrix = zeros(num_true, num_est);
for i = 1:num_true
for j = 1:num_est
dist_matrix(i,j) = norm(true_landmarks(:,i) - est_landmarks(:,j));
end
end
% 简单最近邻匹配
[~, assignment] = min(dist_matrix, [], 2);
% 计算平均误差
errors = zeros(num_true, 1);
for i = 1:num_true
errors(i) = dist_matrix(i, assignment(i));
end
map_accuracy = mean(errors);
fprintf('地图精度: %.4f m\n', map_accuracy);
end七、实用建议
7.1 调试技巧
- 逐步验证：先验证运动模型，再验证观测模型
- 可视化调试：实时绘制协方差椭圆和不确定性
- 单元测试：为每个函数编写测试用例
- 数值稳定性 ：使用
chol分解代替直接求逆
7.2 参数调优
matlab
% 参数敏感性分析
function analyzeParameterSensitivity()
parameters = {'过程噪声', '观测噪声', '地标数量', '传感器范围'};
errors = zeros(length(parameters), 10);
for p = 1:length(parameters)
for trial = 1:10
% 调整参数并运行仿真
% 记录误差
end
end
% 绘制敏感性分析图
figure;
boxplot(errors', 'Labels', parameters);
xlabel('参数'); ylabel('位置误差(m)');
title('参数敏感性分析');
end7.3 实时性能优化
- 降低状态维度：使用子图分割或滑动窗口
- 近似更新：使用信息滤波器或稀疏EKF
- 硬件加速：使用MATLAB Coder生成C代码
- 异步处理：将预测和更新步骤分离
这个完整的EKF-SLAM MATLAB仿真实现包含了从基础到高级的所有内容，你可以根据自己的需求进行修改和扩展。建议先从简化版本开始理解算法流程，再逐步尝试完整版本和高级功能。