clc;clear all;close all;
T=4; %采样周期
N=100/T; %总的采样次数
r=1.01;var_u=1;var_w=3; %几个参数初始化赋值
A=[r,0,0,0;0,r,0,0;T*(1+r)/2,0,1,0;0,T*(1+r)/2,0,1];
B=[1,0,T/2,0;0,1,0,T/2]';
Q=var_u*eye(2);
H=[0,0,1,0;0,0,0,1];
%%%第二问引入传感器误差,引入改变之后的矩阵
var_d=2; %传感器方差
var_n=var_d+var_w;%M[2|2]中出现的
p11=(T^2*var_u+8*r^2*var_n)/(T^2*(1+r)^2);
p13=2*r*var_n/(T*(1+r));
M22=[p11,0,p13,0;0,p11,0,p13;p13,0,var_n,0;0,p13,0,var_n];
% Skf_n=zeros(4,N); %卡尔曼滤波结果
% Skf_n(:,1)=S(:,1);
%{
p11=(T^2*var_u+8*r^2*var_w)/(T^2*(1+r)^2);
p13=2*r*var_w/(T*(1+r));
M22=[p11,0,p13,0;0,p11,0,p13;p13,0,var_w,0;0,p13,0,var_w];
%}
xd=zeros(2,N);
S=zeros(4,N); %信号矢量vx,vy,x,y
S(:,1)=[1,1,1,1]; %x四维,初始绝对位置(1,1)水平速度2,垂直速度2 %(:,1)取第一列
Z=zeros(2,N);
Z(:,1)=[S(3,1),S(4,1)]; %z只观测到位置,没有速度,zx观测值
Z1=zeros(2,N); %z真实相对观测值
Z1(:,1)=Z(:,1); %z只观测到位置,没有速度,zx观测值
Skf=zeros(4,N); %卡尔曼滤波结果
Skf(:,1)=S(:,1);
%%%运动模型,状态模型s(n)=As(n-1)+Bn,和观测到的Z
for i = 2:N
S(:,i)=A*S(:,i-1)+B*var_u*randn(2,1);
Z(:,i)=H*S(:,i)+var_n*randn(2,1);
xd(:,i)=xd(:,i-1)+[1;0]; %沿着x轴匀速
Z1(:,i)=Z(:,i)-xd(:,i); %观测真实相对值
end
zx1=Z(1,1);zx2=Z(1,2);zy1=Z(2,1);zy2=Z(2,2);
db=2*r/(T*(1+r));
Skf(:,2)=[db*(zx2-zx1),db*(zy2-zy1),zx2,zy2];%s^[2|2],两点法
%%%滤波的
for i=3:N
S_pre=A*Skf(:,i-1); %s[k|k-1]=As[k-1|k-1]
M_pre=A*M22*A'+var_n*(B*B'); %M[k|k-1]=AM[k-1|k-1]A'+var_w*BB'
K=M_pre*H'/(H*M_pre*H'+var_n*eye(2)); %K[k]=M[k|k-1]H'(HM[k|k-1]H'+var_w*I)-1
Skf(:,i)=S_pre+K*(Z(:,i)-H*S_pre);
M22=(eye(4)-K*H)*M_pre; %M[k|k]=(I-KH)*M[k|k-1]
end
%%%绘图
figure;
hold on;
plot(S(3,:),S(4,:),'-k'); %位置轨迹
plot(Z(1,:),Z(2,:),'-b.'); %观测轨迹
% plot(Z1(1,:),Z1(2,:),'-m*'); %真实位置
plot(Skf(3,:),Skf(4,:),'-r+'); %Kalman估计轨迹
hold off;legend('真实轨迹','观测轨迹','滤波轨迹')
%%%误差计算模块
for i=1:N
Err_Observation(i)=RMS(S(:,i),Z(:,i));
Err_KalmanFilter(i)=RMS(Skf(:,i),Z(:,i));
end
figure;
hold on; box on;
plot(Err_Observation,'-ko','MarkerFace','g')
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
legend('滤波前误差','滤波后误差')
![avatar](https://profile-avatar.csdnimg.cn/838ed1cb6ced4363956f63f6987512a4_ak_626.jpg!1)
程序任
- 粉丝: 4
- 资源: 1
最新资源
- p019基于Hadoop的租房数据分析系统的设计与实现-flask.zip(Python毕设源码+sql文件+文档)
- p019基于Hadoop的租房数据分析系统的设计与实现-flask.zip(Python毕设源码+sql文件+文档)
- JavaScript互动教程系统.zip
- 数据库酒店管理信息系统报告+文件 - 基于SQL Server的酒店管理系统课程设计
- 数据库酒店管理信息系统报告+文件 - 基于SQL Server的酒店管理系统课程设计
- 数据库酒店管理信息系统报告+文件.基于SQL Server的酒店管理系统课程设计
- AOP下载下来就能用,亲测好用
- go1.23.4.windows-amd64.msi安装包
- Docker技术中私有镜像库创建与管理详解
- Skype-8.134.76.202.dmg
- winf实验十(1).zip
- 基于java的工会会员管理系统源代码(完整前后端+mysql+说明文档+LW).zip
- 基于java的大学生综合评测系统源代码(完整前后端+mysql+说明文档+LW).zip
- 嵌入式的c语言的开发环境
- 基于java的大学新生报到管理系统有前台源代码(完整前后端+mysql+说明文档+LW).zip
- 基于java的电子商品销售网站源代码(完整前后端+mysql+说明文档+LW).zip
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
![feedback](https://img-home.csdnimg.cn/images/20220527035711.png)
![feedback](https://img-home.csdnimg.cn/images/20220527035711.png)
![feedback-tip](https://img-home.csdnimg.cn/images/20220527035111.png)