问题介绍
由于在对导弹的实际姿态和位置的观察中推测导弹的当前状态,常常需要借助扩展卡尔曼滤波器(EKF) 进行滤波处理。
基本问题及算法
下面以一个具体的例子来说明EKF
的在制导过程中的使用:
求解代码
clc,clear
delta_t = 0.01;%T:采样周期
lambda = 10000;
tf = 3.7;%总时间
T =tf/delta_t;%共采样次数
%以下是状态转移矩阵F
F = [eye(3),eye(3)*delta_t,(exp(-lambda*delta_t)+lambda*delta_t-1)/(lambda)^2*eye(3);...
zeros(3,3),eye(3),(1-exp(-lambda*delta_t))/lambda*eye(3);...
zeros(3,3),zeros(3,3),exp(-lambda*delta_t)*eye(3)];
G = [-(delta_t^2/2)*eye(3);-delta_t*eye(3);zeros(3,3)];
N = 3;%制导律
numbers = 50;%仿真的次数
sigma = sqrt(200);%过程状态方差
u = 10*ones(3,T);%存储位置
for i = 1:numbers
x = zeros(9,T);%状态X
x(:,1) = [3500,1500,1000,-1100,-150,-50,10,10,10]';%导弹初始状态
realx(:,1) = x(:,1);
ex = zeros(9,T);
ex(:,1) = [3300,1300,800,-950,-100,-100,0,0,0]';%滤波值的初始状态
w = [zeros(6,T);sigma*randn(3,T)];%过程噪声
Q = [zeros(6),zeros(6,3);zeros(3,6),sigma^2*eye(3)];
z = zeros(2,T);
z(:,1) = [atan(x(2,1)/sqrt(x(1,1)^2+x(3,1)^2)),atan(-x(3,1)/x(1,1))]';
v = zeros(2,T);
for k = 2:T-3
%以下开始实验制导律u的过程
tgo = tf-k*0.01+1e-10;
c1 = N/tgo^2;
c2 = N/tgo;
c3 = N*(exp(-lambda*tgo)+lambda*tgo-1)/(lambda*tgo)^2;
u(1,k-1) = [c1 c2 c3]*[x(1,k-1) x(4,k-1) x(7,k-1)]';
u(2,k-1) = [c1 c2 c3]*[x(2,k-1) x(5,k-1) x(8,k-1)]';
u(3,k-1) = [c1 c2 c3]*[x(3,k-1) x(6,k-1) x(9,k-1)]';
realx(:,k) = F*x(:,k-1)+G*u(:,k-1);
x(:,k) = F*x(:,k-1)+G*u(:,k-1)+w(:,k-1);
d = sqrt(x(1,k)^2+x(2,k)^2+x(3,k)^2);
D = [d 0;0 d];
R = inv(D)*0.1*eye(2)*inv(D)';
v(:,k) = sqrtm(R)*randn(2,1);
z(:,k) = [atan(x(2,k)/sqrt(x(1,k)^2+x(3,k)^2)),atan(-x(3,k)/x(1,k))]'+v(:,k);
end
% 下面根据z(:,k)观测值进行滤波
P0 = [10^4*eye(6),zeros(6,3);zeros(3,6),10^2*eye(3)];%状态协方差初始化
eP0 = P0;
stop = 0.5/0.01;
span = 1/0.01;
for k = 2:T-3
dd = sqrt(ex(1,k-1)^2+ex(2,k-1)^2+ex(3,k-1)^2);
DD = [dd 0;0 dd];
RR = 0.1*eye(2)/(DD*DD');
tgo = tf-k*0.01+1e-10;
c1 = N/tgo^2;
c2 = N/tgo;
c3 = N*(exp(-lambda*tgo)+lambda*tgo-1)/(lambda*tgo)^2;
u(1,k-1) = [c1 c2 c3]*[ex(1,k-1) ex(4,k-1) ex(7,k-1)]';
u(2,k-1) = [c1 c2 c3]*[ex(2,k-1) ex(5,k-1) ex(8,k-1)]';
u(3,k-1) = [c1 c2 c3]*[ex(3,k-1) ex(6,k-1) ex(9,k-1)]';
%以下再进行Extened Kalman Filter得到
[ex(:,k),eP0] = ekf(F,G,Q,RR,eP0,u(:,k-1),z(:,k),ex(:,k-1));
end
%以下开始计算误差
for t = 1:T-3
Ep_ekf(i,t) = sqrt((ex(1,t)-x(1,t))^2+(ex(2,t)-x(2,t))^2+(ex(3,t)-x(3,t))^2);
Ev_ekf(i,t) = sqrt((ex(4,t)-x(4,t))^2+(ex(5,t)-x(5,t))^2+(ex(6,t)-x(6,t))^2);
Ea_ekf(i,t) = sqrt((ex(7,t)-x(7,t))^2+(ex(8,t)-x(8,t))^2+(ex(9,t)-x(9,t))^2);
end
for t = 1:T-3
error_r(t) = mean(Ep_ekf(:,t));
error_v(t) = mean(Ev_ekf(:,t));
error_a(t) = mean(Ea_ekf(:,t));
end
end
%以下开始绘制导弹位置曲线对比图
figure(1)
hold on;box on; grid on;
plot3(realx(1,:),realx(2,:),realx(3,:),'-b.');
plot3(x(1,:),x(2,:),x(3,:),'-k.');
plot3(ex(1,:),ex(2,:),ex(3,:),'-r*','MarkerFace','r');
legend('真实值','滤波前带状态噪声','EKF滤波后');
view(3);xlabel('x/m');ylabel('y/m');zlabel('z/m');
t = 0.01:0.01:3.67;
figure(2)%位置偏差图
hold on;grid on;
plot(t,error_r,'-g.');
xlabel('飞行时间/s');
ylabel('相对位置估计偏差/m');
title('位置偏差图');
figure(3)%速度偏差图
hold on;grid on;
plot(t,error_v,'-k.');
xlabel('飞行时间/s');
ylabel('速度估计偏差/m/s');
title('速度偏差图');
figure(4)%加速度偏差图
hold on;grid on;
plot(t,error_a,'-b.');
xlabel('飞行时间/s');
ylabel('加速度速度估计偏差/m/s');
title('加速度偏差图');
function [ex,P0] = ekf(F,G,Q,R,P0,u,z,ex)
X_ = F*ex+G*u;
Z_ = [atan(X_(2)/sqrt(X_(1)^2+X_(3)^2));atan(-X_(3)/X_(1))];
P = F*P0*F'+ Q;
%以下是计算观测H的雅可比矩阵
dh1_x = -X_(1)*X_(2)/sqrt(X_(1)^2+X_(2)^2+X_(3)^2);
dh1_y = sqrt(X_(1)^2+X_(3)^2)/sqrt(X_(1)^2+X_(2)^2+X_(3)^2);
dh1_z = -X_(2)*X_(3)/(X_(1)^2+X_(2)^2+X_(3)^2);
dh2_x = X_(1)/(X_(1)^2+X_(3)^2);
dh2_z = -X_(1)/(X_(1)^2+X_(3)^2);
H = [dh1_x,dh1_y,dh1_z,zeros(1,6);...
dh2_x,0,dh2_z,zeros(1,6)];
%以上是计算观测H的雅可比矩阵
K = P*H'/(H*P*H'+R);
ex = X_ + K*(z-Z_);
P0 = (eye(9) - K*H)*P;
end
结果
位置曲线: