前言

在前面的问题中我们已经考虑到了用微分方程来描述卫星运动轨迹的方法:

r ¨ = r θ ˙ 2 − G M r − 2 θ ¨ = − 2 r − 1 r ˙ θ ˙ \ddot r = r\dot \theta^2-GMr^{-2}\\\ddot{\theta}=-2r^{-1}\dot r\dot \theta r¨=rθ˙2−GMr−2θ¨=−2r−1r˙θ˙当其运动的参数为: x = [ r , r ˙ , θ , θ ˙ ] T x=[r,\dot r,\theta,\dot{\theta}]^T x=[r,r˙,θ,θ˙]T时,其基本的运动的状态方程为: x ˙ = [ x ( 2 ) x ( 1 ) ∗ ( x ( 4 ) ) 2 − G M ( x ( 1 ) ) − 2 x ( 4 ) − 2 ( x ( 1 ) ) − 1 x ( 2 ) x ( 4 ) ] \dot{x}=\begin{bmatrix}x(2)\\x(1)*(x(4))^2-GM(x(1))^{-2} \\x(4) \\-2(x(1))^{-1}x(2)x(4) \end{bmatrix}\\ x˙=⎣⎢⎢⎡​x(2)x(1)∗(x(4))2−GM(x(1))−2x(4)−2(x(1))−1x(2)x(4)​⎦⎥⎥⎤​
可以采用ode45函数来求解上面的方程实现仿真卫星的真实轨迹的操作。

新的问题及建模

​ 在实际问题中,往往会出现这样一种问题,就是由于其他行星或者天体或者一些其他假设因素的影响,导致卫星的实际运动轨迹并不总是满足上面提到的微分方程的模型。考虑这个过程中 r r r的状态噪声 ζ r \zeta_r ζr​，以及 θ \theta θ的状态噪声 ζ θ \zeta_{\theta} ζθ​的存在。同时我们借助实际的观测仪器进行 r , θ r,\theta r,θ观测时,也会出现观测上的误差 η r , η θ \eta_r,\eta_{\theta} ηr​,ηθ​。因此我们的状态微分方程可以进行以下的改写:
r ¨ = r θ ˙ 2 − G M r − 2 + ζ r θ ¨ = − 2 r − 1 r ˙ θ ˙ + r − 1 ζ θ \ddot r = r\dot \theta^2-GMr^{-2}+\zeta_r\\ \ddot{\theta}=-2r^{-1}\dot r \dot \theta+r^{-1}\zeta_{\theta} r¨=rθ˙2−GMr−2+ζr​θ¨=−2r−1r˙θ˙+r−1ζθ​设 x = [ r , r ˙ , θ , θ ˙ ] T x=[r,\dot r,\theta,\dot{\theta}]^T x=[r,r˙,θ,θ˙]T, ω = ( ζ r , ζ θ ) T \omega = (\zeta_r,\zeta_{\theta})^T ω=(ζr​,ζθ​)T,而 ω \omega ω的协方差矩阵为 Q Q Q,同时用 x ˙ = x k + 1 − x k h \dot{x}=\frac{x_{k+1}-x_k}{h} x˙=hxk+1​−xk​​来离散化下面的方程:
x ˙ = [ x ( 2 ) x ( 1 ) ∗ ( x ( 4 ) ) 2 − G M ( x ( 1 ) ) − 2 x ( 4 ) − 2 ( x ( 1 ) ) − 1 x ( 2 ) x ( 4 ) ] + [ 0 , 0 1 , 0 0 , 0 0 , 1 x ( 1 ) ] [ ζ r ζ θ ] \dot{x}=\begin{bmatrix}x(2)\\x(1)*(x(4))^2-GM(x(1))^{-2} \\x(4) \\-2(x(1))^{-1}x(2)x(4) \end{bmatrix}+\begin{bmatrix} 0,0\\1,0\\0,0\\0,\frac{1}{x(1)}\end{bmatrix}\begin{bmatrix}\zeta_r \\ \zeta_{\theta} \end{bmatrix}\\ x˙=⎣⎢⎢⎡​x(2)x(1)∗(x(4))2−GM(x(1))−2x(4)−2(x(1))−1x(2)x(4)​⎦⎥⎥⎤​+⎣⎢⎢⎡​0,01,00,00,x(1)1​​⎦⎥⎥⎤​[ζr​ζθ​​]
可以得到离散的状态空间模型:
x k + 1 = f ( x k ) + G k ω k x_{k+1} = f(x_k)+G_k\omega_k xk+1​=f(xk​)+Gk​ωk​其中: f ( x k ) = [ x k ( 1 ) + h x k ( 2 ) x k ( 2 ) + h x k ( 1 ) ∗ ( x k ( 4 ) ) 2 − G M h ( x k ( 1 ) ) − 2 x k ( 3 ) + h x k ( 4 ) x k ( 4 ) − 2 h ( x k ( 1 ) ) − 1 x k ( 2 ) x k ( 4 ) ] G k = [ 0 , 0 h , 0 0 , 0 0 , h x k ( 1 ) ] f(x_k) = \begin{bmatrix} x_k(1)+hx_k(2)\\x_k(2)+hx_k(1)*(x_k(4))^2-GMh(x_k(1))^{-2} \\x_k(3)+hx_k(4) \\x_k(4)-2h(x_k(1))^{-1}x_k(2)x_k(4)\end{bmatrix}\\ G_k = \begin{bmatrix} 0,0\\h,0\\0,0\\0,\frac{h}{x_k(1)}\end{bmatrix} f(xk​)=⎣⎢⎢⎡​xk​(1)+hxk​(2)xk​(2)+hxk​(1)∗(xk​(4))2−GMh(xk​(1))−2xk​(3)+hxk​(4)xk​(4)−2h(xk​(1))−1xk​(2)xk​(4)​⎦⎥⎥⎤​Gk​=⎣⎢⎢⎡​0,0h,00,00,xk​(1)h​​⎦⎥⎥⎤​
而对于测量的方程来说,设 v = ( η r , η θ ) T v= (\eta_{r},\eta_{\theta})^T v=(ηr​,ηθ​)T,其协方差矩阵为 R R R,可以得到实际的考虑误差测量方程:
z k = H x k + v k z_k = Hx_k+v_k zk​=Hxk​+vk​
其中: H = [ 1 0 0 0 0 0 1 0 ] H=\begin{bmatrix} 1&0&0&0\\0&0&1&0\end{bmatrix} H=[10​00​01​00​],而根据EKF的步骤要求出 ∂ f k ∂ x k \frac{\partial{f_k}}{\partial{x_k}} ∂xk​∂fk​​,也就是求出 f ( x k ) f(x_k) f(xk​)的雅可比矩阵:

设 x ^ k − \hat{x}_k^- x^k−​为 x k x_k xk​的先验估计, x ^ k \hat{x}_k x^k​为 x k x_k xk​的后验估计。

∂ f ( x k ) ∂ x k ∣ x k = x ^ k − = [ 1 h 0 0 2 h G M ( x ^ k − ( 1 ) ) 3 + h x ^ k − ( 4 ) 2 1 0 2 h x ^ k − ( 1 ) x ^ k − ( 4 ) 0 0 1 h 2 h x ^ k − ( 2 ) x ^ k − ( 4 ) / x ^ k − ( 1 ) 2 − 2 h x ^ k − ( 4 ) / x ^ k − ( 1 ) 0 1 − 2 h x ^ k − ( 2 ) / x ^ k − ( 1 ) ] \frac{\partial{f(x_k)}}{\partial{x_k}}|_{x_k=\hat{x}_k^-}=\begin{bmatrix} 1&h&0&0\\\frac{2hGM}{(\hat{x}_k^-(1))^3}+h\hat{x}_k^-(4)^2&1&0&2h\hat{x}_k^-(1)\hat{x}_k^-(4)\\0&0&1&h\\ 2h\hat{x}_k^-(2)\hat{x}_k^-(4)/\hat{x}_k^-(1)^2&-2h\hat{x}_k^-(4)/\hat{x}_k^-(1)&0&1-2h\hat{x}_k^-(2)/\hat{x}_k^-(1) \end{bmatrix} ∂xk​∂f(xk​)​∣xk​=x^k−​​=⎣⎢⎢⎡​1(x^k−​(1))32hGM​+hx^k−​(4)202hx^k−​(2)x^k−​(4)/x^k−​(1)2​h10−2hx^k−​(4)/x^k−​(1)​0010​02hx^k−​(1)x^k−​(4)h1−2hx^k−​(2)/x^k−​(1)​⎦⎥⎥⎤​

问题的求解

EKF算法参见上篇博客:拓展卡尔曼滤波器(EKF)的数学推导

其中的参数设置:h=24*3600s,Q = 10^8*diag([1,0.5]),R = 10^8*diag([0.5,1]),x0 = [3.86*10^8;0;0;2*pi/28/3600/24],具体的实现代码:

clc,clear;
rng(8);
%设置部分月球卫星的部分参数
G = 6.67259*10^(-11);
M = 5.965*10^(24);
a = 3.86*10^8;
x0 = [a;0;0;2*pi/28/3600/24];
GM = G*M;
day = 24*3600;
tspan = 0:day/24:28*day;
N = 30;
M = 4;M2 = 2;
%以下将求误差协方差矩阵
delta_Q = 1*10^8;
Q = delta_Q*diag([1,0.5]);
h = day;
delta_R = 1*10^8;%测量误差由于混合了状态估计的噪声影响,其数量级也应该在100000km左右
R = delta_R*diag([0.5,1]);
%以下对卫星轨道进行仿真操作
X_real  = zeros(M,N);
Z_real =  zeros(M2,N);
w = zeros(M,N);
v = zeros(M2,N);
X_real(:,1) = x0;Z_real(:,1) = [x0(1);x0(3)]; 
for j = 2:N
    x = X_real(:,j-1);
    f = [x(1)+h*x(2);...
        x(2)+h*x(1)*(x(4))^2-h*GM/(x(1))^2;...
        x(3)+h*x(4);...
        x(4)-2*h*x(2)*x(4)/x(1)];
    g = [0,0;h,0;0,0;0,h/x(1)];
    H = [1 0 0 0;0 0 1 0];
    w(:,j) = g*sqrtm(Q)*randn(2,1);
    v(:,j) = sqrtm(R)*randn(2,1);
    X_real(:,j) = f;
    Z_real(:,j) = H*x;
end
X_test = X_real + w;
Z_test = Z_real + v;
figure(1)
hold on;grid on;
plot(Z_real(1,:).*cos(Z_real(2,:)),Z_real(1,:).*sin(Z_real(2,:)),'ro');
plot(Z_test(1,:).*cos(Z_test(2,:)),Z_test(1,:).*sin(Z_test(2,:)),'b*');
%以上将对噪声进行扩展卡尔曼滤波
X_ekf = zeros(M,N);Z_ekf = zeros(M2,N);
X_ekf(:,1) = X_test(:,1);
Z_ekf(:,1) = Z_test(:,1);
P0 = diag([10^8,10^7/day,10*pi/180,pi/180]);%不重要
for j = 2:N
   x = X_ekf(:,j-1);
   x_ = [x(1)+h*x(2);...
        x(2)+h*x(1)*(x(4))^2-h*GM/(x(1))^2;...
        x(3)+h*x(4);...
        x(4)-2*h*x(2)*x(4)/x(1)];
   g = [0,0;h,0;0,0;0,h/x(1)];
   H = [1 0 0 0;0 0 1 0];
   %下面是估计状态转移矩阵的雅可比矩阵
   f_dx_ = [1,h,0,0;...
           h*(x_(4))^2+2*h*GM/(x_(1)+eps)^3,1,0,2*h*x_(1)*x_(4);...
           0,0,1,h;...
           2*h*x_(2)*x_(4)/(x_(1)+eps)^2,-2*h*x_(4)/(x_(1)+eps),0,1-2*h*x_(2)/(eps+x_(1))]; 
   P_ =  f_dx_*P0*f_dx_'+ g*Q*g';
   K = P_*H'/(H*P_*H' + R);
   x = x_ + K*(Z_test(:,j) - H*x_);
   P0 = (eye(M)-K*H)*P_;
   X_ekf(:,j) = x;
end
plot(X_ekf(1,:).*cos(X_ekf(3,:)),X_ekf(1,:).*sin(X_ekf(3,:)),'go','MarkerFaceColor','k');
legend('真实值','加入高斯噪声','EKF滤波后');
xlabel('x/m');
ylabel('y/m');
title('卫星轨道在EKF前后对比');

结论

实际上由于原来问题的非线性程度太高了,所以实际滤波效果可以说是非常的差。