%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
kx = .01; ky = .05; % 阻尼系数
g = 9.8; % 重力
t = 10; % 仿真时间
Ts = 0.1; % 采样周期
len = fix(t/Ts); % 仿真步数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%(真实轨迹模拟)
dax = 1.5; day = 1.5; % 系统噪声
X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
x = x + vx*Ts;
vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
y = y + vy*Ts;
vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
X(k,:) = [x, vx, y, vy];
end
figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 构造量测量
mrad = 0.001;
dr = 10; dafa = 10*mrad; % 量测噪声
for k=1:len
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);
Z(k,:) = [r, a];
end
figure(1), hold on, plot(Z(:,1).*sin(Z(:,2)), Z(:,1).*cos(Z(:,2)),'*')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ekf 滤波
Qk = diag([0; dax; 0; day])^2;
Rk = diag([dr; dafa])^2;
Xk = zeros(4,1);
Pk = 100*eye(4);
X_est = X;
for k=1:len
Ft = JacobianF(X(k,:), kx, ky, g);
Hk = JacobianH(X(k,:));
fX = fff(X(k,:), kx, ky, g, Ts);
hfX = hhh(fX, Ts);
[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);
X_est(k,:) = Xk';
end
figure(1), plot(X_est(:,1),X_est(:,3), '+r')
xlabel('X'); ylabel('Y'); title('ekf simulation');
legend('real', 'measurement', 'ekf estimated');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
EKF卡尔曼滤波.rar_PKA_卡尔曼滤波器_扩展卡尔曼_扩展卡曼滤波
版权申诉
5星 · 超过95%的资源 188 浏览量
2022-07-13
22:39:46
上传
评论 1
收藏 15KB RAR 举报
刘良运
- 粉丝: 62
- 资源: 1万+
最新资源
- 基于JavaScript的访客预约系统设计源码
- 基于Vue和ECharts的工作租房数据可视化系统设计源码
- 1040g0cg310ravpiu6ibg5pg00tsipsln3ju2d0g 2
- 基于Python的SAR图像去噪CNN-NLM设计源码
- redhat6升级到redhat7,过程redhat6.x-> redhat6.10->rehat7.9 主版本最高版本
- 基于Django的流程引擎设计源码
- 基于Node.js的Express框架与MySQL的后台管理系统设计源码
- 基于Java的Flink流批一体数据处理快速集成开发框架设计源码
- FirstFilterOrderCompare
- Screenshot_2024-03-28-19-17-25-020_com.ss.android.lark.jpg
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
评论2