标签：real xk EKF 卡尔曼滤波 Matlab bmatrix theta hat dot
前言
在前面的问题中我们已经考虑到了用微分方程来描述卫星运动轨迹的方法:
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=[10000100],而根据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)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: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前后对比');
结论
实际上由于原来问题的非线性程度太高了,所以实际滤波效果可以说是非常的差。
标签：real,xk,EKF,卡尔曼滤波,Matlab,bmatrix,theta,hat,dot 来源： https://blog.csdn.net/shengzimao/article/details/119901195
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。