轨道定轨中的最小二乘法思想（1）

矢志航天的阿洪

已于 2024-04-02 21:37:03 修改

阅读量887

点赞数 21

文章标签：算法

于 2024-04-02 21:32:26 首次发布

本文链接：https://blog.csdn.net/weixin_57997461/article/details/137245098

版权

本文探讨了如何利用最小二乘方法估计系统状态，通过一个雷达追踪自由落体的案例，解释了状态模型、观测模型、噪声处理以及状态估计的过程。通过线性化和迭代更新，逐步优化初始估计，直到达到收敛状态。

摘要由CSDN通过智能技术生成

一、基本思想

系统动力学模型（状态模型或状态方程）：

$\dot{X}(t)=f(X(t),t)+w(t)$

其中， $X(t)$ 为系统状态变量; $w(t)$ 为系统模型噪声，通常假设为零均值高斯白噪声。

系统观测模型（观测方程）

$z_i=h(X(t_i))+v_i,i=1,2,...,N$

其中， $z_i$ 为系统观测量， $v_i$ 为系统观测噪声，也通常假设为零均值高斯白噪声。

假设已获得系统在 $t_1,t_2,...,t_N$ 时刻观测量 $z_1,z_2,...,z_N$ ，就可以估计 $t_0$ 时刻的系统状态 $X(t_0)=X_0$ 。假设初始估计值为 $\hat{X}(t_0)=\hat{X_0}$ ,根据系统微分方程 $\dot{X}(t)=f[X(t),t]+w(t)$ ，得到 $t_1,t_2,...,t_N$ 的状态变量 $\hat{X}(t_1),\hat{X}(t_2),...,\hat{X}(t_i),...,\hat{X}(t_N)$ ，然后根据观测量与状态变量的关系，得到该估计的系统状态方程 $z_i=h[X(t_i)]+v_i,i=1,2,...,N$ ，得到估计观测量的值 $\hat{z}(t_1),\hat{z}(t_2),...,\hat{z}(t_i),\hat{z}(t_N)$

残差：实际观测量与估计观测量之间偏差，即 $z(t_i)-\hat{z}(t_i)$ .

最小二乘思想：通过最优估计方法，使系统在观测区间内的残差平方和达到最小。定义使残差平方和最小的估计指标为：

$J=\sum_{i=1}^{N}\left \| z_i-\hat{z_i} \right \|^2=\sum_{i=1}^{N}\left \| z_i-h(\hat{X}(t_i)) \right \|^2=\sum_{i=1}^{N}\left \| z_i-h(\hat{g}(\hat{X_0,}t_i)) \right \|^2$

令：

$Z=\begin{bmatrix}z_1 \\ z_2 \\ . \\ . \\ . \\ z_N \end{bmatrix},H(\hat{X_0})=\begin{bmatrix}\hat{z}_1 \\ \hat{z}_2 \\ . \\ . \\ . \\ \hat{z}_N \end{bmatrix}$

则估计指标可表示为：

$J(\hat{X_0})=\sum_{i=1}^{N}\left \| z_i-h(g(\hat{X_0},t_i)) \right \|=[Z-H(\hat{X_0})]^T[Z-H(\hat{X_0})]$

若定义系统的最小二乘估计为 $\hat{X}_{0}^{*}$ ,可使估计指标达到全局极小值，即满足：

$J(\hat{X_{0}^{*}})=minJ(\hat{X_0})$

但通常 $\hat{X_0},\hat{Z}$ 通常是隐性的函数关系， $[Z-H(\hat{X_0})]^T\frac{\partial H(\hat{X_0})}{\partial \hat{X_n}}=0$ 为高度非线性方程，难于求解

例如：假设在原点o处有一部雷达，水平方向距离雷达 $l$ 处有一枚炸弹，从高度 $h_0$ 以初速度 $\dot{h_0}$ 自由下落，雷达以20Hz的采样频率，输出对该炸弹的观测量 $\left ( t_i,\rho _i,E_i \right )$ ，该系统的状态方程和观测方程为

系统状态向量： $X=[h,\dot{h}]^T$

系统动力学模型：

$\frac{dX}{dt}=\begin{bmatrix}0 & 1\\ 0 & 0 \end{bmatrix}X+\begin{bmatrix}0 \\ -g \end{bmatrix}+\begin{bmatrix} w_h\\ w_{\dot{h}} \end{bmatrix}$

系统观测模型：

$z_i=\begin{bmatrix} \sqrt{h_{i}^{2}+l^2}\\tan^{-1}\left ( \frac{h_i}{l} \right ) \end{bmatrix}+\begin{bmatrix} v_{\rho}\\v_{E} \end{bmatrix},i=1,2,...,N$

目的：通过观测量 $\left ( t_i,\rho_i,E_i \right )$ ，估计炸弹的状态向量 $[h(t_0),\dot{h}(t_0)]$

相互独立： $E[w_hw_{\dot{h}}]=0$ ，零均值高斯白噪声：

零均值： $E[w_h]=0,E[w_{\dot{h}}]=0$

高斯：概率密度函数是正态函数

白噪声： $E[w_h(t_i)w_{h}(t_j)]=0$ ， $E[w_{\dot{h}}(t_i)w_{\dot{h}}(t_j)]=0$

根据自由落体运动规律

$\hat{h}(t)=\hat{h}(t_0)+\hat{\dot{h}}(t_0)(t-t_0)-\frac{g(t-t_0)^2}{2}$

$\hat{\dot{h}}(t)=\hat{\dot{h}}(t_0)-g(t-t_0)$

代入观测方程中得到的 $\frac{\partial H(\hat{X}_0)}{\partial \hat{X}_0}$ 形式非常复杂。

根据系统观测方程：

$z_i=\begin{bmatrix} \rho_i\\E_i \end{bmatrix}=h(X_i)+\begin{bmatrix} v_\rho\\v_E \end{bmatrix}=\begin{bmatrix} \sqrt{h_i^2+l^2}\\ tan^{-1}(\frac{h_i}{l}) \end{bmatrix}+\begin{bmatrix} v_\rho\\v_E \end{bmatrix}$

求解系统观测矩阵：

$\tilde{h}(X)=\frac{\partial h}{\partial X}=\begin{bmatrix} \frac{\partial \rho}{\partial h} &\frac{\partial \rho}{\partial \dot{h}} \\ \frac{\partial E}{\partial h} & \frac{\partial E}{\partial \dot{h}} \end{bmatrix}=\begin{bmatrix} \frac{h}{\sqrt{h^2+l^2}} &0 \\ \frac {l}{h^2+l^2} & 0 \end{bmatrix}$

根据先验知识，得到的状态估计量为 $\hat{X}_i=[\hat{h}_i,\hat{\dot{h}}_i]$ ，代入系统观测矩阵

$\tilde{h}_i=\begin{bmatrix} \frac{\hat{h_i}}{\sqrt{\hat{h_i^2}+l^2}}&0 \\ \frac{l}{\hat{h}_i^2+l^2}& 0 \end{bmatrix}$

设预估状态和真实状态之差为

$\Delta{X}_i=X_i-\hat{X}_i=[\Delta h_{i},\Delta{\dot{h}_i}]^T=[h_i-\hat{h}_i,\dot{h}_i-\hat{\dot{h}}_i]^T$

观测方程及其线性化后

$\hat{h}_i\Delta {X_i}=\begin{bmatrix} \frac{h_i}{\sqrt{\hat{h}_i^2+l^2}}&0 \\\frac{l}{\hat{h_i^2}+l^2} & 0 \end{bmatrix} \begin{bmatrix} \Delta{h_i}\\\Delta{\dot{h_i}} \end{bmatrix} =\begin{bmatrix} \frac{h_i\Delta {h_i}}{\sqrt{\hat{h_i^2}+l^2}}\\\frac{l\Delta{\dot{h_i}}}{\hat{h}_i^2+l^2} \end{bmatrix}$

根据给观测的数据和先验状态下得到的观测值

$z_i=\begin{bmatrix} \rho_i\\E_i \end{bmatrix}$

$h(\hat{X_i})=\begin{bmatrix} \sqrt{\hat{h_i^2}+l^2}\\ tan^{-1}\frac{\hat{h_i}}{l} \end{bmatrix}$

得到以下等式

$\begin{bmatrix} \rho_i-\sqrt{\hat{h_i^2}+l^2}\\ E_i-tan^{-1}(\frac{\hat{h_i}}{l}) \end{bmatrix}=\begin{bmatrix} \frac{\hat{h_i}\Delta{h_i}}{\sqrt{\hat{h_i^2+l^2}}}\\ \frac{l\Delta\dot{h_i}}{\hat{h_i^2}+l^2} \end{bmatrix}+v_i$

根据系统状态方程：

$\frac{dX}{dt}=\begin{bmatrix} 0 &1 \\ 0&0 \end{bmatrix}X+\begin{bmatrix} 0\\-g \end{bmatrix}+\begin{bmatrix} w_\rho\\w_E \end{bmatrix}$

得到状态转移矩阵为

$\Phi(t_i,t_0)=\begin{bmatrix} 1 &(t-t_i) \\ 0 & 1 \end{bmatrix}$

假设第k-1次迭代后，自由落体初始状态估计为 $\hat{X}_0$ ，则第k次迭代的状态微分修正量 $\Delta{X}_0$ ，满足微分修正条件方程：

$\hat{h_i}\Phi(t_i,t_0)\Delta{X_0}=z_i-h(\hat{X_i}),i=1,2,...,N$

那么根据最小二乘原理，求解出 $\Delta {X_0}$ ，然后将求出的值对求出的先验估计进行修正

$\hat{X_{0}^{+}}=\hat{X_{0}^{-}}+\Delta \hat{X}_0$

然后采用同样的方式进行上述操作，进行新一轮迭代，当修正量足够小时，即认为该值为估计值。

二、案例实践

1、首先假设真实情况下目标的初始高度为 $(20m,0m/s)$ ，观测位置在水平位置的分量为5m，观测时刻为 $t_1,t_2,...,t_{10}$ ，步长为0.2s，开始观测时间为0.2s，根据系统状态转移方程，得到其观测得到的值，代码如下

g=9.8;                %重力加速度
h0=20;                %假设其真实高度为20m
l=5;                  %水平位置为5m
v0=0;                 %真实初始速度为0m/s
t=0:0.2:2;            %得到观测时间
X=zeros(2,length(t));
rou=zeros(1,length(t));
E=zeros(1,length(t));
%% 写成该系统的动力学方程
X0=[h0;v0];
for i=1:length(t)
    phi=[1 t(i);0 1];            %状态转移矩阵
    const=[-g*(t(i)^2)/2;-g*t(i)];
    X(:,i)=phi*X0+const;         %状态转移方程
    rou(i)=sqrt(X(1,i)^2+l^2)+10e-4*randn(1); %得到真实观测的距离
    E(i)=atan(X(1,i)/l)+10e-4*randn(1);       %得到真实观测的俯仰角   
end

2、设计先验估计值 $\hat {X_0}$ 为 $(25m,0m/s)$ ，根据先验估计值，代入状态转移方程和观测方程，得到其观测值与估计观测值的差，代码如下：

hat_X0=[25;0];hat_X=zeros(2,length(t));
hat_rou=zeros(1,length(t)-1);
hat_E=zeros(1,length(t)-1);
zz=zeros(1,2*(length(t)-1));
for i=2:length(t)-1
    phi=[1 t(i);0 1];
    const=[-g*(t(i)^2)/2;-g*t(i)];
    hat_X(:,i)=phi*hat_X0+const;%% 根据先验值得到的每个时刻的状态值
    hat_rou(i)=sqrt(hat_X(1,i)^2+l^2)+10e-4*randn(1);
    hat_E(i)=atan(hat_X(1,i)/l)+10e-4*randn(1);
    zz((i-1)*2-1)=rou(i)-hat_rou(i); 
    zz((i-1)*2)=E(i)-hat_E(i); 
end
z=zz';%得到最小二乘法中对应的Z项

3、根据上文介绍的线性化方法，求出每个时刻的对应的H矩阵，例如： $t_1$ 状态量 $X_1=(\hat{h}_1,\hat{\dot{h}}_1)$ ，则 $\frac{\partial h}{\partial X}$ ，其对应的雅可比矩阵

$\tilde{h}_1=\begin{bmatrix} \frac{\hat{h_1}}{\sqrt{\hat{h_1^2}+l^2}}&0 \\ \frac{l}{\hat{h}_1^2+l^2}& 0 \end{bmatrix}$

该时刻的状态转移方程为

$\Phi(t_1,t_0)=\begin{bmatrix} 1 &t_1 \\ 0 & 1 \end{bmatrix}$

通过两个式子相乘得到 $\Delta {X_0}$ 对应的系数矩阵 $H$ ，根据最小二乘法原理，使残差最小，得到估计值 $\Delta {\hat{X_0}}=(H^TH)^{-1}(H^TZ)$

H=zeros(2*(length(t)-1),2);
for m=2:length(t)
    X1=hat_X(:,m);
    Jac=[X1(1)/sqrt(X1(1)^2+l^2) 0;l/(X1(1)^2+l^2) 0];
    Phi=[1 t(m);0 1];
    h=Jac*Phi;
    H((m-1)*2-1,:)=h(1,:);
    H((m-1)*2,:)=h(2,:);
end

Hat_Delta_X0=inv(H'*H)*(H'*z)

求解出的结果为 $\Delta {\hat{X}}_0=(-5.16,0.29)$

4、将求出的修正值代入先验估计中，重新进行上一步操作，得到的第二次修正结果

$\Delta {\hat{X}}_0=(0.15 ,-0.28)$ ，进行第三次迭代，得到第三次的修正结果为 $\Delta {\hat{X}}_0=(0.01 ,-0.01)$ ，修正量较小，停止迭代，得到最终条件为 $(20m,0m/s)$ 。整个问题的代码在下面给出

clc;clear
%% 假设目标从20m高度往下扔
%% 距离横向长度为5m，观测10次数据 采样间隔为1s
g=9.8;
h0=20;
l=5;
v0=0;
t=0:0.2:2;
X=zeros(2,length(t));
rou=zeros(1,length(t));
E=zeros(1,length(t));
%% 写成该系统的动力学方程
X0=[h0;v0];
for i=1:length(t)
    phi=[1 t(i);0 1];
    const=[-g*(t(i)^2)/2;-g*t(i)];
    X(:,i)=phi*X0+const;
    rou(i)=sqrt(X(1,i)^2+l^2)+10e-4*randn(1);
    E(i)=atan(X(1,i)/l)+10e-4*randn(1);
end
%% 估计X0的初始位置
Hat_Delta_X0=[10;10];hat_X0=[25;0];
while abs(Hat_Delta_X0(1))>0.01 && abs(Hat_Delta_X0(2))>0.01
    
    hat_X=zeros(2,length(t));
    hat_rou=zeros(1,length(t)-1);
    hat_E=zeros(1,length(t)-1);
    zz=zeros(1,2*(length(t)-1));
    for i=2:length(t)-1
        phi=[1 t(i);0 1];
        const=[-g*(t(i)^2)/2;-g*t(i)];
        hat_X(:,i)=phi*hat_X0+const;%% 根据先验值得到的每个时刻的状态值
        hat_rou(i)=sqrt(hat_X(1,i)^2+l^2)+10e-4*randn(1);
        hat_E(i)=atan(hat_X(1,i)/l)+10e-4*randn(1);
        zz((i-1)*2-1)=rou(i)-hat_rou(i);
        zz((i-1)*2)=E(i)-hat_E(i);
    end
    %对于h1时刻的状态
    z=zz';H=zeros(2*(length(t)-1),2);
    for m=2:length(t)
        X1=hat_X(:,m);
        Jac=[X1(1)/sqrt(X1(1)^2+l^2) 0;l/(X1(1)^2+l^2) 0];
        Phi=[1 t(m);0 1];
        h=Jac*Phi;
        H((m-1)*2-1,:)=h(1,:);
        H((m-1)*2,:)=h(2,:);
    end
    
    Hat_Delta_X0=inv(H'*H)*(H'*z);
    hat_X0=hat_X0+Hat_Delta_X0
end

矢志航天的阿洪

关注

21
点赞
踩
22

收藏

觉得还不错? 一键收藏
1
评论
轨道定轨中的最小二乘法思想（1）

系统动力学模型（状态模型或状态方程）：其中，为系统状态变量;为系统模型噪声，通常假设为零均值高斯白噪声。系统观测模型（观测方程）其中，为系统观测量，为系统观测噪声，也通常假设为零均值高斯白噪声。假设已获得系统在时刻观测量，就可以估计时刻的系统状态。假设初始估计值为,根据系统微分方程，得到的状态变量，然后根据观测量与状态变量的关系，得到该估计的系统状态方程，得到估计观测量的值残差：实际观测量与估计观测量之间偏差，即。
复制链接

扫一扫