24小时热门版块排行榜

返回列表

zhwei19800

新虫 (初入文坛)

应助: 0 (幼儿园)
金币: 3
帖子: 5
在线: 7小时
虫号: 3740126
注册: 2015-03-15
专业: 控制理论与方法

[求助] 水平抛物的扩展卡尔曼滤波，我程序或者理解在哪出了问题，错误在哪里？求高人指点已有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

回复此楼

» 猜你喜欢

博士读完未来一定会好吗已经有38人回复
谈谈两天一夜的“延安行” 已经有3人回复
之前让一硕士生水了7个发明专利，现在这7个获批发明专利的维护费可从哪儿支出哈？已经有11人回复
氨基封端PDMS和HDI反应快速固化已经有10人回复
论文投稿求助已经有4人回复
Applied Surface Science 这个期刊。有哪位虫友投过的能把word模板发给我参考一下嘛已经有3人回复
投稿精细化工已经有6人回复
博士申请都是内定的吗？已经有9人回复
心脉受损已经有8人回复

1楼2015-03-20 19:27:13

已阅回复此楼关注TA 给TA发消息送TA红花 TA的回帖

zhwei19800

新虫 (初入文坛)

应助: 0 (幼儿园)
金币: 3
帖子: 5
在线: 7小时
虫号: 3740126
注册: 2015-03-15
专业: 控制理论与方法

我又改了下。
这条语句：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')%实际曲线