EKF在三维寻的制导系统中的应用

问题介绍

由于在对导弹的实际姿态和位置的观察中推测导弹的当前状态,常常需要借助扩展卡尔曼滤波器(EKF) 进行滤波处理。

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

结果

位置曲线:

位置误差曲线

速度误差曲线

加速度误差曲线