| 查看: 1097 | 回复: 2 | |||
[求助]
水平抛物的扩展卡尔曼滤波，我程序或者理解在哪出了问题，错误在哪里？求高人指点已有1人参与
|
|
我自己做了一个matlab程序，但是仿出来，估计曲线根本收敛不到实际曲线。我想请明白的朋友给分析分析，是程序有问题（卡尔曼滤波方程） ，还是什么问题。如果能帮忙给修改过来，我感激不尽。 1、问题：从空中水平抛射出的物体，初始水平速度vx ，初始位置坐标（0,0 ）；受重力 和阻尼力影响，阻尼力与速度平方成正比，水平和垂直阻尼；还存在不确定的零均值白噪声干扰力。在坐标原点处有一观测设备（不妨想象成雷达），可测得距离 （零均值白噪声误差 ）、角度 （零均值白噪声误差 ）。 2、程序 clear all kx = 0.1; ky =0.5; % 阻尼系数 g = 9.8; % 重力 t = 20; % 仿真时间 Ts = 0.1; % 采样周期 len = fix(t/Ts); % 仿真步数 dax = 1.5; day = 1.5; % 系统噪声 mrad = 0.001; dr = 10; dafa = 10*mrad; % 量测噪声 X=[0;50;500;0];%初始位置 %初始化 P_pre=100*eye(4); Q = diag([0; dax; 0; day])^2; R = diag([dr; dafa])^2; X_pre=[0 ; 50;520;0];%初始估计初值 X_est=[0 ; 50;500;0];%最终估计值，用于演示 U=[0 ;0 ;0 ;-g]; %实际曲线 for i=2:len X(:,i)=[1 Ts 0 0;0 1-kx*X(2,(i-1))*Ts 0 0; 0 0 1 Ts;0 0 0 1+ky*X(4,(i-1))*Ts]*X(:,(i-1))+[0 ;0 ;0; -g*Ts]+[0;dax*randn(1,1)*Ts;0 ;day*randn(1,1)*Ts]; Y(:,i)=[sqrt(X(1,(i))^2+X(3,(i))^2); atan(X(1,(i))/X(3,(i))) ]+[dr*randn(1,1);dafa*randn(1,1)]; end for i=1  len-1)F=[1 Ts 0 0;0 1-2*kx*X_pre(2,(i))*Ts 0 0; 0 0 1 Ts;0 0 0 1+2*ky*X_pre(4,(i))*Ts];%用上一时刻后验估计 X_pre(:,i+1)=X_pre(:,i)+([X_pre(2,i);-kx*X_pre(2,i)^2;X_pre(4,i);ky*X_pre(4,i)^2]+U)*Ts; P_pre(:,:,i+1)=F*P_pre(:,:,i)*F'*Ts+Q;%为啥要乘以Ts H=[X_pre(1,(i+1))/sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0 X_pre(3,(i+1))/sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0;X_pre(3,(i+1))/(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0 -X_pre(1,(i+1))/(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0];%用当前时刻先验估计 %H=[1/sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0 1/sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0;X_pre(3,(i+1))/(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0 -X_pre(1,(i+1))/(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0];%用当前时刻先验估计 K=P_pre(:,:,i+1)*H'*(H*P_pre(:,:,i+1)*H'+R)^-1; X_post(:,i+1)=X_pre(:,i+1)+K*(Y(:,i+1)-[sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2); atan(X_pre(1,(i+1))/X_pre(3,(i+1))) ]); P_post(:,:,i+1)=P_pre(:,:,i+1)-K*H*P_pre(:,:,i+1); %赋值 P=P_post(:,:,i+1);%保存每次的后验估计 X_est(:,i+1)=X_post(:,i+1);%保存每次的后验估计 P_pre(:,:,i+1)=P_post(:,:,i+1); X_pre(:,i+1)=X_post(:,i+1); end plot(X(1,  ,X(3,, 'r',X_est(1,,X_est(3,,'g') 1.jpg |
回复此楼» 猜你喜欢
谈谈两天一夜的“延安行”
已经有6人回复
-
博士申请都是内定的吗？
已经有12人回复
-
氨基封端PDMS和HDI反应快速固化
已经有11人回复
-
之前让一硕士生水了7个发明专利，现在这7个获批发明专利的维护费可从哪儿支出哈？
已经有11人回复
-
论文投稿求助
已经有4人回复
-
Applied Surface Science 这个期刊。有哪位虫友投过的能把word模板发给我参考一下嘛
已经有3人回复
-
投稿精细化工
已经有6人回复
|
我又改了下。 这条语句：P_pre(:,:,i+1)=(F-eye(4))*P_pre(:,:,i)*(F-eye(4))'+Q;%这里为啥不用F=A+I 而用A呢。用A就能估计，用F就发散。咋回事。 clear all kx = 0.1; ky =0.5; % 阻尼系数 g = 9.8; % 重力 t = 5; % 仿真时间 Ts = 0.1; % 采样周期 len = fix(t/Ts); % 仿真步数 dax = 1.5; day = 1.5; % 系统噪声 mrad = 0.001; dr = 10; dafa = 10*mrad; % 量测噪声 X=[0;50;500;0];%初始位置 %初始化 P_pre=100*eye(4); P_post=100*eye(4); L=[0 0 0 0;0 1 0 0;0 0 0 0;0 0 0 1]; Q = L*diag([0; dax; 0; day])^2*L'; R =[1 0;0 1] *diag([dr; dafa])^2*[1 0;0 1]'; Pk = 100*eye(4); X_pre=[0 ; 50;500;0]; X_post=[0 ; 50;500;0]; X_est=[0 ; 50;500;0]; U=[0 ;0 ;0 ;-g*Ts]; %实际曲线 for i=1:len X(:,i+1)=[1 Ts 0 0;0 1-kx*X(2,(i))*Ts 0 0; 0 0 1 Ts;0 0 0 1+ky*X(4,(i))*Ts]*X(:,(i))+[0 ;0 ;0; -g*Ts]+[0;dax*randn(1,1)*Ts;0 ;day*randn(1,1)*Ts]; Y(:,i+1)=[sqrt(X(1,(i+1))^2+X(3,(i+1))^2); atan(X(1,(i+1))/X(3,(i+1)))]+[dr*randn(1,1);dafa*randn(1,1)]; end X(1, ;X(3, ;for i=1:len F=[1 Ts 0 0;0 1-2*kx*X_pre(2,(i))*Ts 0 0; 0 0 1 Ts;0 0 0 1+2*ky*X_pre(4,(i))*Ts];%用上一时刻后验估计 %X_pre(:,i+1)=[1 Ts 0 0;0 1-kx*X_pre(2,(i))*Ts 0 0; 0 0 1 Ts;0 0 0 1+ky*X_pre(4,(i))*Ts]*X_pre(:,(i))+U; X_pre(:,i+1)=X_pre(:,i)+([X_pre(2,i);-kx*X_pre(2,i)^2;X_pre(4,i);ky*X_pre(4,i)^2])*Ts+U; %X_pre(:,i+1)=F*X_pre(:,i)+U; P_pre(:,:,i+1)=(F-eye(4))*P_pre(:,:,i)*(F-eye(4))'+Q;%这里为啥不用F=A+I %P_pre(:,:,i+1)=F*P_pre(:,:,i)*(F)'+Q; %H=[1/sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0 1/sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0;X_pre(3,(i+1))/(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0 -X_pre(1,(i+1))/(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0]; H=[X_pre(1,(i+1))/sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0 X_pre(3,(i+1))/sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0;X_pre(3,(i+1))/(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0 -X_pre(1,(i+1))/(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2) 0];%用当前时刻先验估计 K=P_pre(:,:,i+1)*H'*inv(H*P_pre(:,:,i+1)*H'+R); X_post(:,i+1)=X_pre(:,i+1)+K*(Y(:,i+1)-[sqrt(X_pre(1,(i+1))^2+X_pre(3,(i+1))^2); atan(X_pre(1,(i+1))/X_pre(3,(i+1))) ]); P_post(:,:,i+1)=P_pre(:,:,i+1)-K*H*P_pre(:,:,i+1); %赋值 P=P_post(:,:,i+1); X_est(:,i+1)=X_post(:,i+1); P_pre(:,:,i+1)=P_post(:,:,i+1); X_pre(:,i+1)=X_post(:,i+1); end plot(X(1, ,X(3,, 'r',X_est(1,,X_est(3,,'+r')%实际曲线 |
2楼2015-03-22 10:23:29
mchen_6431
专家顾问 (著名写手)
-
专家经验: +168 - 应助: 181 (高中生)
- 金币: 15763.8
- 散金: 35
- 红花: 127
- 帖子: 2018
- 在线: 837.9小时
- 虫号: 837980
- 注册: 2009-09-01
- 性别: GG
- 专业: 计算机软件
- 管辖: 信息科学
3楼2015-03-22 17:38:37