卫星轨道的估计问题(Matlab)(二):扩展卡尔曼滤波(EKF)对新问题的尝试_赛亚茂的博客-程序员ITS304_ekf matlab

技术标签: matlab  参数估计技术  算法  线性代数  

前言

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

在这里插入图片描述

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θ˙2GMr2θ¨=2r1r˙θ˙当其运动的参数为: 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))2GM(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θ˙2GMr2+ζrθ¨=2r1r˙θ˙+r1ζθ 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+1xk来离散化下面的方程:
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))2GM(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))2GMh(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=[10000100],而根据EKF的步骤要求出 ∂ f k ∂ x k \frac{\partial{f_k}}{\partial{x_k}} xkfk,也就是求出 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} xkf(xk)xk=x^k=1(x^k(1))32hGM+hx^k(4)202hx^k(2)x^k(4)/x^k(1)2h102hx^k(4)/x^k(1)001002hx^k(1)x^k(4)h12hx^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前后对比');

在这里插入图片描述

结论

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

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/shengzimao/article/details/119901195

智能推荐

JDK、JRE和JVM之间的关系_Sun-yz的博客-程序员ITS304_jdk jre jvm三者之间的关系

作为一个Java开发者,只会用Java,却不知什么是JDK、JRE和JVM是什么,以及他们之间有什么联系。本文总结了JDK,JRE,JVM三者的关系与区别。JDK、JRE和JVM之间的关系一、JDK二、JRE三、JVM四、三者的联系五、三者的区别六、总结一、JDKJDK是Java开发工具包,其中包括编译工具(javac.exe)打包工具(jar.exe)等,也包括JRE。在JDK的安装目录下有一个jre目录,里面有两个文件夹bin和lib,在这里可以认为bin里的就是jvm,lib中则是jvm工.

初进csdn,写点什么!!!_知知脏的博客-程序员ITS304

以前都是在本站上面当伸手党,我已经觉悟了,要回馈了!!!!!从今往后,我会把自己所总结/踩过的坑,分享出来,让大家遇到和我同样困难的时候,也可以少走弯路,一起成长学习。小白刚上路,大神勿喷!!!!!!!...

如何编写技术白皮书_coofive的博客-程序员ITS304_技术白皮书格式

下面这篇文章介绍了技术白皮书的由来,以及如何编写:http://www.stelzner.com/copy-HowTo-whitepapers.php

UIScrollView的总结_大燕codeblog的博客-程序员ITS304

概述UIScrollView可以展示比设备屏幕更大区域的内容,我们可以通过手指滑动来查看内容视图的每一部分内容,也可以通过手指捏合来对内容视图进行缩放操作,它是 TableView和 UITextView的父类。属性与方法 注: 本文中所说的”内容视图”在官方文档中称作”content view”,表示UIScrollView中可以用来展示内容的部分内容视图相关// 内容视图的大小,默认

三层交换机的配置命令详解_llhh33的博客-程序员ITS304_三层交换机配置命令

什么是三层交换机三层交换机就是具有部分路由器功能的交换机,三层交换机的最重要目的是加快大型局域网内部的数据交换,所具有的路由功能也是为这目的服务的,能够做到一次路由,多次转发。对于数据包转发等规律性的过程由硬件高速实现,而像路由信息更新、路由表维护、路由计算、路由确定等功能,由软件实现。三层交换技术就是二层交换技术+三层转发技术。传统交换技术是在OSI网络标准模型第二层——数据链路层进行操作的,...

Element分页handleCurrentChange方法触发问题解决_qq_35049655的博客-程序员ITS304

先操作element分页,切换到第二页,然后操作。从代码中强制将current-page强制设置为1(即重新查询数据,并将当前页重置为第1页)此时画面显示是对的,分页组件已经将第1页的页码数字激活了然后点击第2页数字,进行换页画面显示也是对的,第2页数字变成激活状态,但是此时竟然无法触发current-change事件原因使用this.pagination.currentP...

随便推点

论文阅读笔记2_weixin_36919892的博客-程序员ITS304

作者信息:Sofiyanti Nery这个作者写的论文不是很多,但是他的个人主页上有很多关于深度学习模型的解释和自己做的若干实验,是少数几个把深度学习讲的特别清楚透彻的人了。 发表日期:2015,2 发表期刊/会议:IEEE Transactions on Neural Networks &Learning Systems。 被引量:27主要内容:这篇文章指出CNN存在的问题:神经网络不能像人

Qt Style Sheet实践(二):组合框QComboBox的定制_weixin_34176694的博客-程序员ITS304

导读     组合框是一个重要且应用广泛的组件,一般由两个子组件组成:文本下拉单部分和按钮部分。在许多既需要用户选择、又需要用户手动输入的应用场景下,组合框能够很好的满足我们的需求。如我们经常使用的聊天软件QQ登录框,便是一个很好的应用例子:     显然,用户既可以自己手动输入新的QQ号码,也可以在列表框中选择历史输入记录。对于提高用户体验是一个不错的手段。这篇博文重点讲述如何用QS...

vSphere安装和设置(一)_狼鬼霸天的博客-程序员ITS304

vSphere安装和设置和简介 vSphere6.5提供各种安装配置选项。 vSphere 的两个核心组件是 ESXi 和 vCenter Server。 ESXi 是用于创建和运行虚拟机及虚拟设备的虚拟化平台。vCenter Server 是一种服务, 充当连接到网络的 ESXi 主机的中心管理员。 vCenter Server 可用于将多个主机的资源加入池中并管理这些资源。

JVM NativeMemoryTracking ;jcmd process_id VM.native_memory;Native memory tracking is not enabled_himal-himal的博客-程序员ITS304

查看原生内存信息:jcmd process_id VM.native_memory summaryjcmd <pid> VM.native_memory [summary | detail | baseline | summary.diff | detail.diff | shutdown]1.执行命令:jcmd <pid> VM.native_memory detail2.

简单Java swing登录gui_T神的博客-程序员ITS304_java用swing建立登录界面

import javax.swing.JButton; import javax.swing.JFrame;import javax.swing.JLabel;import javax.swing.JPanel;import javax.swing.JPasswordField;import javax.swing.JTextField;public class test { p...

Android进阶:多线程断点续传下载_BianChengNinHao的博客-程序员ITS304

今天跟大家一起分享下android开发中比较难的一个环节,可能很多人看到这个标题就会感觉头很大,的确如果没有良好的编码能力和逻辑思维,这块是很难搞明白的.什么是多线程下载?多线程下载其实就是迅雷,BT一些下载原理,通过多个线程同时和服务器连接,那么你就可以榨取到较高的带宽了,大致做法是将文件切割成N块,每块交给单独一个线程去下载,各自下载完成后将文件块组合成一个文件,程序上要完成做

推荐文章

热门文章

相关标签