% function EXT Kalman Filtering
% vx[n]=vx[n-1]+ux[n]
% vy[n]=vy[n-1]+uy[n]
% rx[n]=rx[n-1]+vx[n-1]*t
% ry[n]=ry[n-1]+vy[n-1]*t
% s[n]=[rx[n];ry[n];vx[n];vy[n]]
% A=[1 0 t 0;0 1 0 t;0 0 1 0;0 0 0 1]
% s[n-1]=[rx[n-1];ry[n-1];vx[n-1];vy[n-1]]
% u[n]=[0;0;ux[n];uy[n]]
% s[n]=As*[n-1]+u[n]
% x[n]=h(s[n])+w[n]
% initialization
clear;
n=100;
A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
rx=zeros(1,n); % 实际位置
ry=zeros(1,n); % 实际位置
vx=zeros(1,n); % 实际速度
vy=zeros(1,n); % 实际速度
rxe=zeros(1,n); % 精确位置
rye=zeros(1,n); % 精确位置
vxe=zeros(1,n); % 精确速度
vye=zeros(1,n); % 精确速度
rx(1,1)=10; % 位置初始
ry(1,1)=-5; % 位置初始
vx(1,1)=-0.2; % 速度初始
vy(1,1)=0.2; % 速度初始
rxe(1,1)=10;
rye(1,1)=-5;
vxe(1,1)=-0.2;
vye(1,1)=0.2;
s=[rx;ry;vx;vy];
se=[rxe;rye;vxe;vye];
r=zeros(1,n);
b=zeros(1,n);
rn=zeros(1,n);
re=zeros(1,n);
bn=zeros(1,n);
x=[rn;bn];
H=zeros(2,4,n);
ux=sqrt(0.00001)*randn(1,n);
uy=sqrt(0.00001)*randn(1,n);
wr=sqrt(0.001)*randn(1,n);
wb=sqrt(0.0001)*randn(1,n);
Q=[0 0 0 0;0 0 0 0;0 0 cov(ux) 0;0 0 0 cov(uy)]; % 过程噪声矩阵
C=[cov(wr) 0; 0 cov(wb)]; % 观测噪声矩阵
ss=zeros(4,n);% 精确值
sn=zeros(4,n); % 估计修正值
sn(:,1)=sn(:,1)+[15;5;0;0]; % 估计修正初始值
ss=zeros(4,n); % 一步预测
hs=zeros(2,n); % 观测值预测
inn=zeros(2,n); % 新息
M=zeros(4,4,n); % 估计均方误差
M(:,:,1)=M(:,:,1)+[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; % 估计均方误差初始值
MM=zeros(4,4,n); % 一步预测均方误差
K=zeros(4,2,n); % 卡尔曼增益
% kalman filtering
for t=1:n-1
% s[n]=As*[n-1]+u[n]
% x[n]=h(s[n])+w[n]
vx(1,t+1)=vx(1,t)+ux(1,t+1); % 速度过程方程
vy(1,t+1)=vy(1,t)+uy(1,t+1); % 速度过程方程
rx(1,t+1)=rx(1,t)+vx(1,t); % 位置过程方程
ry(1,t+1)=ry(1,t)+vy(1,t); % 位置过程方程
s(:,t+1)=[rx(1,t+1);ry(1,t+1);vx(1,t+1);vy(1,t+1)]; % 过程矩阵,维数4*1
vxe(1,t+1)=vxe(1,t); % 理论精确值
vye(1,t+1)=vye(1,t); % 理论精确值
rxe(1,t+1)=rxe(1,t)+vxe(1,t); % 理论精确值
rye(1,t+1)=rye(1,t)+vye(1,t); % 理论精确值
se(:,t+1)=[rxe(1,t+1);rye(1,t+1);vxe(1,t+1);vye(1,t+1)]; % 过程矩阵,维数4*1
r(1,t+1)=(rx(1,t+1)^2+ry(1,t+1)^2)^(1/2); % 理论测量距离(含测量误差)
b(1,t+1)=atan(ry(1,t+1)/rx(1,t+1)); % 理论测量方位(含测量误差)
rn(1,t+1)=r(1,t+1)+wr(1,t+1); % 实际测量距离
re(1,t+1)=(rxe(1,t+1)^2+rye(1,t+1)^2)^(1/2); % 理论测量距离(含测量误差)
bn(1,t+1)=b(1,t+1)+wb(1,t+1); % 实际测量方位
x(:,t+1)=[rn(1,t+1);bn(1,t+1)]; % 观测矩阵,维数2*1
% 对观测方程求导,得到雅可比矩阵H,维数2*4
H(:,:,t+1)=[rx(1,t+1)/((rx(1,t+1)^2+ry(1,t+1)^2)^(1/2)) ry(1,t+1)/((rx(1,t+1)^2+ry(1,t+1)^2)^(1/2)) 0 0;-ry(1,t+1)/(rx(1,t+1)^2+ry(1,t+1)^2) rx(1,t+1)/(rx(1,t+1)^2+ry(1,t+1)^2) 0 0];
MM(:,:,t+1)=A*M(:,:,t)*A'+Q; % 一步预测均方误差
K(:,:,t+1)=MM(:,:,t+1)*H(:,:,t+1)'/(C+H(:,:,t+1)*MM(:,:,t+1)*H(:,:,t+1)'); % 卡尔曼增益
M(:,:,t+1)=(eye(4)-K(:,:,t+1)*H(:,:,t+1))*MM(:,:,t+1);% 估计均方误差
ss(:,t+1)=A*sn(:,t); % 一步预测
hs(:,t+1)=[((ss(1,t+1))^2+(ss(2,t+1))^2)^(1/2);atan(ss(2,t+1)/ss(1,t+1))]; % 观测值预测
inn(:,t+1)=x(:,t+1)-hs(:,t+1); % 新息
sn(:,t+1)=ss(:,t+1)+K(:,:,t+1)*inn(:,t+1); % 预测修正
end
subplot(2,1,1);
plot(sn(1,:),sn(2,:)); % kalman估计值
hold on;
plot(se(1,:),se(2,:),'-r'); % 理论精确值
subplot(2,1,2);
plot(rn); % 实际测量距离
hold on;
plot(re,'-g'); % 理论精确值
没有合适的资源?快使用搜索试试~ 我知道了~
温馨提示
【达摩老生出品,必属精品,亲测校正,质量保证】 资源名:matlab一个四维状态、二维观测的目标跟踪扩展卡尔曼滤波程序,附有详细的说明 资源类型:matlab项目全套源码 源码说明: 全部项目源码都是经过测试校正后百分百成功运行的,如果您下载后不能运行可联系我进行指导或者更换。 适合人群:新手及有一定经验的开发人员
资源推荐
资源详情
资源评论
收起资源包目录
matlab一个四维状态、二维观测的目标跟踪扩展卡尔曼滤波程序,附有详细的说明.rar (1个子文件)
ekf_4d.m 3KB
共 1 条
- 1
资源评论
- WXT9900572024-02-09感谢资源主的分享,这个资源对我来说很有用,内容描述详尽,值得借鉴。
- 吴小葉2023-06-10资源有一定的参考价值,与资源描述一致,很实用,能够借鉴的部分挺多的,值得下载。
- fenfen9232022-09-01感谢资源主的分享,很值得参考学习,资源价值较高,支持!
阿里matlab建模师
- 粉丝: 3208
- 资源: 2782
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功