[发明专利]基于自适应迭代粒子滤波的小卫星姿态估计方法

说明书

技术领域

本发明涉及一种小卫星姿态估计方法，特别是涉及一种基于自适应迭代粒子滤波的小卫星姿态估计方法。

背景技术

确定卫星姿态就是根据姿态敏感器的测量值，获得该卫星相对于基准方位的矢量信息来确定卫星的姿态。卫星姿态确定的方法之一是状态估计法，该方法本质上就是非线性滤波问题，从概率统计最优的角度估计出小卫星的姿态。但是卫星姿态确定自身模型及其复杂，再加上其它原因，导致卫星的系统方程具有强非线性。

文献“基于Unscented四元数粒子滤波的微小卫星姿态估计，北京航空航天大学学报，2007，Vol33(5)，P552-556”，公开了一种基于Unscented四元数粒子滤波的微小卫星姿态估计方法。该方法以误差四元数为姿态参数，设计了一种基于UPF的姿态估计器，用MEMS陀螺采集的随机噪声数据进行了计算机半物理仿真。Unscented粒子滤波(UPF)通过非线性变换得到采样点的近似状态分布，这样解决了系统的非线性问题，该算法是将UKF嵌入到粒子滤波的框架里，通过这种方法计算粒子滤波的重要性采样密度，由于该重要密度函数中包含最新量测信息，与传统的PF方法相比具有更好的性能。文献所述方法其不足之处是不适于噪声非高斯分布的系统，也没有自适应，这些都对所得结果的精度有影响。因为采样粒子的过程用到了UKF算法，所以采样效率也不高，计算量反而增加了。

发明内容

为了克服现有小卫星姿态估计方法实用性差的不足，本发明提供一种基于自适应迭代粒子滤波的小卫星姿态估计方法。该方法当重要性密度函数的高斯分布的期望和方差，经过计算超出一定的阈值，这时利用修正项自身迭代的方法，实时更新重要密度函数，使得粒子可以迅速推进到高似然区域，这也让粒子样本的权重均匀分散，提高了采样的效率，并且同时运用模拟退火算法，解决了粒子权重较小、不易归一化的问题，由此产生自适应迭代粒子滤波(AIPF)方法。本发明把这个算法应用于小卫星系统的姿态估计中，使得能够高精度的估计卫星姿态。

本发明解决其技术问题所采用的技术方案是：一种基于自适应迭代粒子滤波的小卫星姿态估计方法，其特点是包括以下步骤：

步骤一：对状态初始值和状态初始方差值进行初始化，分别给定初始值x₁和P₁。

步骤二：进行时间更新，选取一个转移概率密度作为初始重要性概率密度函数p(x_t+1|x_t)，并且从中抽取粒子样本其中，N表示粒子总数，x_t表示为t时刻的状态，表示为t时刻第i个粒子样本。

步骤三：进行量测更新。

①判断步骤二抽取的粒子样本是否有效；

把抽取的粒子样本带入到量测方程得出概率密度函数p(y_t|z_1：t-1)服从正态分布表示t时刻第i个粒子带入量测方程所得的量测值，表示在t时刻第i个粒子在量测方程中进行非线性计算；

其中为t-1时刻第i-1个粒子的权重；

若上式计算出的和p_t满足ρ是给定的临界值，则该粒子有效；若不满足，通过粒子滤波的多次迭代计算修正项c(z_t)；

其中，是x_t经过k-1次迭代后所得估计值，z_t是实际测量值。只要c_k(z_t)已知，就用c_k(z_t)代替c(z_t)。同时采用模拟退火方法解决似然函数呈尖峰状态的问题，扩大量测噪声的方差为αR，α≥1，其中R是量测噪声的方差，α是扩大噪声的系数。得出c(z_t)后，得到一个新的重要性概率密度函数再从中获得新的粒子样本其中p_v为系统噪声服从的概率密度函数，x_t表示t时刻的状态，f(x_t-1)表示t-1时刻的状态在系统方程中进行非线性计算。

②计算权值权值归一化其中，为似然函数，为转移概率密度函数，为重要概率密度函数。

步骤四：当粒子出现退化，对其进行重采样。

步骤五：计算系统状态的估计值计算状态方差

步骤六：令t＝t+1，再回到步骤二，进行下一个时刻的循环，直到t＝T，T为设置的时间循环次数。