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

权利要求书

1.一种基于自适应迭代粒子滤波的小卫星姿态估计方法，其特征在于包括以下步骤：

步骤一：对状态初始值和状态初始方差值进行初始化，分别给定初始值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)；

ck(zt)=0,k=1x^t,k-1-f(x^t-1),k>1]]>

其中，是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为设置的时间循环次数。