嘿,亲!知识可是无价之宝呢,但咱这精心整理的资料也耗费了不少心血呀。小小地破费一下,绝对物超所值哦!如有下载和支付问题,请联系我们QQ(微信同号):813200300
本次赞助数额为: 2 元微信扫码支付:2 元
请留下您的邮箱,我们将在2小时内将文件发到您的邮箱
使用Matlab实现卡尔曼滤波的轨迹预测
% GPS精度为1m、气压计精度为0.5m,加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行,半径为 20m,螺距为40m,100s完成一圈飞行
% 数据采集频率为 10Hz
clear all
D = 1; % 维度,1
dt = 0.1; % 0.1s采集一次数据
t0 = 0:dt:100; % 0~100s
N = length(t0); % 采样点数
A = eye(D); % 状态转移矩阵,和上一时刻状态没有换算,故取 D阶单位矩阵,eye(1), 1阶单位阵 = 1
x = zeros(D, N); % 存储滤波后的数据
z = ones(D, N); % 存储滤波前的数据
x(:, 1) = ones(D,1); % 初始值设为 1(可为任意数)
P = eye(D); % 初始值为 1(可为非零任意数),取 D阶单位矩阵
Q = 0.01*eye(D); % 过程噪声协方差,估计一个
R = 1; % 测量噪声协方差,精度为多少取多少
k = 4; % 采样点计数
true1D = t0*0.4; % 一维仅高度,气压计数据
for t = dt:dt:100
k = k 1;
x(:,k) = A * x(:,k-1); % 卡尔曼公式1
P = A * P * A' Q; % 卡尔曼公式2
H = eye(D);
K = P*H' * inv(H*P*H' R); % 卡尔曼公式3
z(:,k) = true1D(k) randn; % 一维仅高度(z方向)
x(:,k) = x(:,k) K * (z(:,k)-H*x(:,k)); % 卡尔曼公式4
P = (eye(D)-K*H) * P; % 卡尔曼公式5
end
plot(t0, z,'b.'); % 绘制滤波前数据
axis('equal');hold on;grid on; % 坐标等距、继续绘图、添加网格
plot(t0, x,'r.'); % 绘制滤波后数据
plot(t0, true1D ,'k-'); % 绘制真实值
legend('滤波前','滤波后','理想值'); % 标注
xlabel('时间: s');
ylabel('高度: m');hold off;
下面是1~3维都可以使用的代码,其实添加的内容不多,跟一维很类似,只不过多了几个条件判断
% GPS精度为1m、气压计精度为0.5m,加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行,半径为 20m,螺距为40m,100s完成一圈飞行
% 数据采集频率为 10Hz
clear all
D = 3; % 维度,可取 1,2,3
dt = 0.1; % 0.1s采集一次数据
t0 = 0:dt:100; % 0~100s
N = length(t0); % 采样点数
A = eye(D); % 状态转移矩阵,和上一时刻状态没有换算,故取 D阶单位矩阵
x = zeros(D, N); % 存储滤波后的数据
z = ones(D, N); % 存储滤波前的数据
x(:, 1) = ones(D,1); % 初始值设为 1(可为任意数)
P = eye(D); % 初始值为 1(可为非零任意数),取 D阶单位矩阵
r = 20; % 绕圈半径,20m
w = 2*pi / 100; % 计算出角速度,100s绕一圈
Q = 1e-2*eye(D); % 过程噪声协方差,估计一个
R = [1 0 0;
0 1 0;
0 0 0.5]; % 测量噪声协方差,精度为多少取多少
k = 1; % 采样点计数
if D==1 % 一维仅高度,气压计数据
true1D = t0*0.4;
elseif D==2 % 二维 x,y 方向,GPS数据
true2D = [r*cos(w*t0); r*sin(w*t0)];
elseif D==3 % 三维 x,y,z方向,GPS和气压计
true3D = [r * cos(w*t0); r * sin(w*t0); t0 * 0.4];
end
for t = dt:dt:100
k = k 1;
x(:,k) = A * x(:,k-1); % 卡尔曼公式1
P = A * P * A' Q; % 卡尔曼公式2
H = eye(D);
K = P*H' * inv(H*P*H' R); % 卡尔曼公式3
if D==1 % 一维仅高度(z方向)
z(:,k) = true1D(k) randn;
elseif D==2 % 二维 x,y 方向
z(:,k) = [true2D(1,k) randn; true2D(2,k) randn];
elseif D==3 % 三维 x,y,z 方向
z(:,k) = [true3D(1,k) randn; true3D(2,k) randn; true3D(3,k) randn];
end
x(:,k) = x(:,k) K * (z(:,k)-H*x(:,k)); % 卡尔曼公式4
P = (eye(D)-K*H) * P; % 卡尔曼公式5
end
if D==1 %% 一维情况
plot(t0, z,'b.'); % 绘制滤波前数据
axis('equal');hold on;grid on; % 坐标等距、继续绘图、添加网格
plot(t0, x,'r.'); % 绘制滤波后数据
plot(t0, true1D ,'k-'); % 绘制真实值
legend('滤波前','滤波后','理想值'); % 标注
xlabel('时间: s');
ylabel('高度: m');hold off;
elseif D==2 %% 二维情况
plot(z(1,:),z(2,:),'b.'); % 绘制滤波前数据
axis('equal');grid on;hold on; % 坐标等距、继续绘图、添加网格
plot(x(1,:),x(2,:),'r.'); % 绘制滤波后数据
plot(true2D(1,:), true2D(2,:), 'k.'); % 绘制真实值
legend('滤波前','滤波后','理想值');
xlabel('x方向: m');
ylabel('y方向: m');hold off;
elseif D==3 %% 三维情况
plot3(z(1,:),z(2,:),z(3,:),'b.'); % 绘制滤波前数据
axis('equal');grid on;hold on % 坐标等距、继续绘图、添加网格
plot3(x(1,:),x(2,:),x(3,:),'r.'); % 绘制滤波后数据
plot3(true3D(1,:), true3D(2,:), true3D(3,:));% 绘制滤波后数据
legend('滤波前','滤波后','理想值'); % 绘制真实值
xlabel('x方向: m');
ylabel('y方向: m');
zlabel('高度: m');hold off;
end