前言
在前面的问题中我们已经考虑到了用微分方程来描述卫星运动轨迹的方法:
r¨=rθ˙2−GMr−2θ¨=−2r−1r˙θ˙\ddot r = r\dot \theta^2-GMr^{-2}\\\ddot{\theta}=-2r^{-1}\dot r\dot \thetar¨=rθ˙2−GMr−2θ¨=−2r−1r˙θ˙当其运动的参数为:x=[r,r˙,θ,θ˙]Tx=[r,\dot r,\theta,\dot{\theta}]^Tx=[r,r˙,θ,θ˙]T时,其基本的运动的状态方程为:x˙=[x(2)x(1)∗(x(4))2−GM(x(1))−2x(4)−2(x(1))−1x(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函数来求解上面的方程实现仿真卫星的真实轨迹的操作。
新的问题及建模
在实际问题中,往往会出现这样一种问题,就是由于其他行星或者天体或者一些其他假设因素的影响,导致卫星的实际运动轨迹并不总是满足上面提到的微分方程的模型。考虑这个过程中rrr的状态噪声ζr\zeta_rζr，以及θ\thetaθ的状态噪声ζθ\zeta_{\theta}ζθ的存在。同时我们借助实际的观测仪器进行r,θr,\thetar,θ观测时,也会出现观测上的误差ηr,ηθ\eta_r,\eta_{\theta}ηr,ηθ。因此我们的状态微分方程可以进行以下的改写:
r¨=rθ˙2−GMr−2+ζrθ¨=−2r−1r˙θ˙+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˙,θ,θ˙]Tx=[r,\dot r,\theta,\dot{\theta}]^Tx=[r,r˙,θ,θ˙]T,ω=(ζr,ζθ)T\omega = (\zeta_r,\zeta_{\theta})^Tω=(ζr,ζθ)T,而ω\omegaω的协方差矩阵为QQQ,同时用x˙=xk+1−xkh\dot{x}=\frac{x_{k+1}-x_k}{h}x˙=hxk+1−xk来离散化下面的方程:
x˙=[x(2)x(1)∗(x(4))2−GM(x(1))−2x(4)−2(x(1))−1x(2)x(4)]+[0,01,00,00,1x(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ζθ]
可以得到离散的状态空间模型:
xk+1=f(xk)+Gkωkx_{k+1} = f(x_k)+G_k\omega_k xk+1=f(xk)+Gkωk其中: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,hxk(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,ηθ)Tv= (\eta_{r},\eta_{\theta})^Tv=(ηr,ηθ)T,其协方差矩阵为RRR,可以得到实际的考虑误差测量方程:
zk=Hxk+vkz_k = Hx_k+v_k zk=Hxk+vk
其中:H=[10000010]H=\begin{bmatrix} 1&0&0&0\\0&0&1&0\end{bmatrix}H=[10000100],而根据EKF的步骤要求出∂fk∂xk\frac{\partial{f_k}}{\partial{x_k}}∂xk∂fk,也就是求出f(xk)f(x_k)f(xk)的雅可比矩阵:
设x^k−\hat{x}_k^-x^k−为xkx_kxk的先验估计,x^k\hat{x}_kx^k为xkx_kxk的后验估计。
∂f(xk)∂xk∣xk=x^k−=[1h002hGM(x^k−(1))3+hx^k−(4)2102hx^k−(1)x^k−(4)001h2hx^k−(2)x^k−(4)/x^k−(1)2−2hx^k−(4)/x^k−(1)01−2hx^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)2h10−2hx^k−(4)/x^k−(1)001002hx^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:Nx = 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;endX_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:Nx = 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;endplot(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前后对比');
结论
实际上由于原来问题的非线性程度太高了,所以实际滤波效果可以说是非常的差。
