CN105242058B - 一种双频弹性干扰下角速度滤波方法 - Google Patents
一种双频弹性干扰下角速度滤波方法 Download PDFInfo
- Publication number
- CN105242058B CN105242058B CN201510828799.8A CN201510828799A CN105242058B CN 105242058 B CN105242058 B CN 105242058B CN 201510828799 A CN201510828799 A CN 201510828799A CN 105242058 B CN105242058 B CN 105242058B
- Authority
- CN
- China
- Prior art keywords
- mtd
- mtr
- msub
- mrow
- mover
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Landscapes
- Gyroscopes (AREA)
Abstract
一种双频弹性干扰下角速度滤波方法,涉及一种角速度滤波方法,特别是涉及一种角速度扩展卡尔曼滤波方法。为了解决现有的传递函数形式滤波器在面临不同频率的弹性干扰时需要重新设计的问题和进行弹性干扰和测量噪声滤除时产生相位延迟而导致的角速度信号估计值不准确的问题。本发明首先针对弹性干扰下角速度信号动态过程进行建模,并建立角速度和干扰信号的状态方程;然后针对状态方程设计状态转移矩阵,同时采用EKF设计Kalman滤波方程,并利用设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行滤波处理。本发明适用于角速度滤波领域。
Description
技术领域
本发明涉及一种角速度滤波方法,特别是涉及一种角速度扩展卡尔曼滤波方法。
背景技术
同陀螺器件相比,角加速度计具有体积小、成本降低空间大、无漂移、安装限制少的特点。角加速度计通常被用来组建无陀螺仪的惯性测量单元(gyro-free inertialmeasurement unit GF-IMU),在航天飞行器中具有很好的应用前景。在一些工程实践中将飞行器作为刚体模型来处理,认为角加速度计只受到测量噪声的影响。实际上,飞行器是一个弹性体,除了测量噪声外还受到飞行器自身弹性振动的影响。这种弹性振动对角速度测量有着很大的影响。针对该问题,在以往工程实践中,将弹性振动建模为不同频率和幅度的正弦信号,并且假定该干扰信号的频率是已知且固定的;干扰信号的幅度和角速度信号幅度的比例关系是已知的,使用传递函数式的数字滤波器对角速度信号的测量值进行滤波。这种方法存在两个缺陷。第一,滤波器传递函数的形式和弹性干扰信号的频率有关,在不同的场合下,弹性干扰的频率可能不同,导致滤波器需要重新设计。这使得该方法适用面比较窄。第二,传递函数形式的滤波器虽然对弹性干扰和测量噪声进行了滤除,但是会产生相位延迟,导致对角速度信号估计值不准确。
发明内容
本发明为了解决现有的传递函数形式滤波器在面临不同频率的弹性干扰时需要重新设计的问题和进行弹性干扰和测量噪声滤除时产生相位延迟而导致的角速度信号估计值不准确的问题。
一种双频弹性干扰下角速度滤波方法,包括下述步骤:
步骤1、针对弹性干扰下角速度信号动态过程进行建模:
飞行器的角速度动态过程为
其中,ω代表角速度,a代表角加速度,M为转动力矩,J为转动惯量;
弹性干扰信号为两个正弦信号叠加而成,设两个正弦信号的频率分别为ω1和ω2,单位为rad/s;两个正弦信号的相位分别为和幅度分别为b1和b2;
弹性干扰信号的形式如下
将弹性干扰信号中的进行处理得到弹性干扰状态v1、v2,将弹性干扰信号中的进行处理得到弹性干扰状态v3、v4;
将干扰频率ω1和ω2当做状态进行估计;设状态为X=[x v1 v2 v3 v4 ω1 ω2]T,其中x为角速度信号,即x=ω;则角速度和干扰信号的状态方程为
其中,az角加速度的测量值;
角速度的测量方程为
z=x+v1+v3+w (4)
式中,z为角速度的测量值;w是零均值高斯白噪声;
测量矩阵为
H=[1 1 0 1 0 0 0] (5)
步骤2、设计状态转移矩阵:
由状态方程(3)描述的系统的状态转移矩阵为
式中
φ2=-T2ω1v1 (8)
φ4=-T2ω1v2-2Tω1v1 (10)
φ6=-T2ω2v3 (12)
φ8=-T2ω2v4-2Tω2v3 (14)
其中,T为测量周期;
步骤3、设计Kalman滤波方程:
角速度滤波器采用EKF(扩展卡尔曼滤波器)设计,Kalman滤波器方程为
Kk+1=Pk+1,kHT(HPk+1,kHT+R)-1 (22)
Pk+1,k+1=(I-Kk+1H)Pk+1,k (24)
式中,是对k+1时刻状态的估计值;是对k+1时刻状态的预报值;Kk和Kk+1分别是k时刻和k+1时刻的滤波器增益;Zk+1为k+1时刻的测量值;Φk+1,k为k时刻到k+1时刻的状态转移矩阵;Pk+1,k为k+1时刻状态预报的协方差矩阵;Pk,k和Pk+1,k+1分别是k时刻和k+1时刻状态估计协方差矩阵;Q为过程噪声矩阵;R为测量噪声矩阵;I为7×7维单位矩阵;
步骤4、根据步骤3设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行滤波处理。
本发明具有以下有益效果:
本发明设计了一种基于扩展卡尔曼滤波的角速度测量信号的滤波方法,适用于两个任意低频正弦弹性干扰下的角速度测量值的干扰和测量噪声滤除问题;设计好的滤波器不需要根据不同的弹性干扰频率重新设计或者修改,可以适用于任意低频弹性干扰信号,使得本发明的滤波方法适用范围较广;而且本发明的滤波方法可以估计出弹性干扰信号的频率。同时本发明的滤波方法针对角速度信号的估计值基本上没有相位滞后,使得估计结果更加准确,估计误差标准差相对于测量误差标准差减小了70%以上。
附图说明
图1为实施例中角速度信号真值、测量值和估计值的对比图;
图2为实施例中弹性干扰状态v1下的估计仿真结果;
图3为实施例中弹性干扰状态v2下的估计仿真结果;
图4为实施例中弹性干扰状态v3下的估计仿真结果;
图5为实施例中弹性干扰状态v4下的估计仿真结果;
图6为实施例中对两个弹性干扰信号频率的估计仿真结果。
具体实施方式
具体实施方式一:
一种双频弹性干扰下角速度滤波方法,包括下述步骤:
步骤1、针对弹性干扰下角速度信号动态过程进行建模:
飞行器的角速度动态过程为
其中,ω代表角速度,a代表角加速度,M为转动力矩,J为转动惯量;
弹性干扰信号为两个正弦信号叠加而成,设两个正弦信号的频率分别为ω1和ω2,单位为rad/s;两个正弦信号的相位分别为和幅度分别为b1和b2;
弹性干扰信号的形式如下
将弹性干扰信号中的进行处理得到弹性干扰状态v1、v2,将弹性干扰信号中的进行处理得到弹性干扰状态v3、v4;
将干扰频率ω1和ω2当做状态进行估计;设状态为X=[x v1 v2 v3 v4 ω1 ω2]T,其中x为角速度信号,即x=ω;则角速度和干扰信号的状态方程为
其中,az角加速度的测量值;
角速度的测量方程为
z=x+v1+v3+w (4)
式中,z为角速度的测量值;w是零均值高斯白噪声;
测量矩阵为
H=[1 1 0 1 0 0 0] (5)
步骤2、设计状态转移矩阵:
由状态方程(3)描述的系统的状态转移矩阵为
式中
φ2=-T2ω1v1 (8)
φ4=-T2ω1v2-2Tω1v1 (10)
φ6=-T2ω2v3 (12)
φ8=-T2ω2v4-2Tω2v3 (14)
其中,T为测量周期;
步骤3、设计Kalman滤波方程:
角速度滤波器采用EKF(扩展卡尔曼滤波器)设计,Kalman滤波器方程为
Kk+1=Pk+1,kHT(HPk+1,kHT+R)-1 (22)
Pk+1,k+1=(I-Kk+1H)Pk+1,k (24)
式中,是对k+1时刻状态的估计值;是对k+1时刻状态的预报值;Kk和Kk+1分别是k时刻和k+1时刻的滤波器增益;Zk+1为k+1时刻的测量值;Φk+1,k为k时刻到k+1时刻的状态转移矩阵;Pk+1,k为k+1时刻状态预报的协方差矩阵;Pk,k和Pk+1,k+1分别是k时刻和k+1时刻状态估计协方差矩阵;Q为过程噪声矩阵;R为测量噪声矩阵;I为7×7维单位矩阵;
步骤4、根据步骤3设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行滤波处理。
具体实施方式二:
本实施方式步骤1所述测量矩阵H通过以下步骤得到:
针对角速度的测量方程有如下形式
z=HX+w
同时,根据角速度的测量方程z=x+v1+v3+w和X=[x v1 v2 v3 v4 ω1 ω2]T,得到H=[1 1 0 1 0 0 0]。
其它步骤及参数与具体实施方式一相同。
具体实施方式三:
本实施方式步骤1所述的将弹性干扰信号中的进行处理得到v1、v2,将弹性干扰信号中的进行处理得到v3、v4的过程如下:
设v2为v1关于时间的一阶导数,则对v2取时间的导数为
同理设v4为v3关于时间的一阶导数,则对v4取时间的导数为
根据上述关系,将其整理成弹性干扰信号状态方程的形式如下
其它步骤及参数与具体实施方式二相同。
实施例
在本实施例中,角速度信号为x=asin(ωt+φ),其中a=0.5,ω=1(rad/s),相位φ为随机值在范围[0,2π)中均匀分布,选取φ=0.7854(rad);两个低频干扰信号角频率分别为v1=b1sin(ω1t+φ1)和v2=b2sin(ω2t+φ2),其中b1=b2=0.1×a,ω1=20×2π(rad/s),ω2=50×2π(rad/s),相位φ1和φ2为随机值在范围[0,2π)中均匀分布,选取φ1=1.5057(rad),φ2=0.0025(rad);角速度滤波器状态初始值为X=[0 0 0 0 0 26 34]T。
根据以上参数,按照具体实施方式三的步骤进行仿真实验,实验结果如图1至图6所示。
图1显示了角速度信号真值、测量值和估计值的比较结果。可以看到测量值受到了比较强烈的弹性振动信号和测量噪声的干扰。本发明的滤波器比较显著的滤除了振动信号和测量噪声的干扰,得到的测量信号的估计值和真值很接近而且信号没有相位上的延迟。图2显示了频率为20Hz的弹性振动信号即弹性干扰状态v1下的估计结果。由于刚开始滤波器设定的振动信号的频率不准确(代表对振动信号频率不具有准确信息),所以在前0.2秒估计结果较差。但很快滤波器的估计结果就向真值收敛。图3,图4和图5分别显示了弹性干扰状态v2,v3和v4下的估计结果,类似于状态v1的估计,在0.2秒后滤波器的估计结果向真值收敛。图6显示了对两个弹性干扰信号频率的估计,对于频率为20Hz的弹性信号,滤波器初始值设置频率为26Hz,对于频率为50Hz的弹性信号,滤波器初始值设置频率为44Hz,代表对弹性干扰信号的频率信息事先并不完全确定。可以看到在0.2秒后,两个频率的估计值就快速趋向真值并稳定在真值上。在本实施例中,测量误差标准差为0.042rad/s,而估计误差的标准差为0.0081rad/s。估计误差的标准差相对于测量误差标准差减小了81%。
Claims (2)
1.一种双频弹性干扰下角速度滤波方法,其特征在于包括下述步骤:
步骤1、针对弹性干扰下角速度信号动态过程进行建模:
飞行器的角速度动态过程为
<mrow>
<mover>
<mi>&omega;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mfrac>
<mi>M</mi>
<mi>J</mi>
</mfrac>
<mo>=</mo>
<mi>a</mi>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,ω代表角速度,a代表角加速度,M为转动力矩,J为转动惯量;
弹性干扰信号为两个正弦信号叠加而成,设两个正弦信号的频率分别为ω1和ω2,单位为rad/s;两个正弦信号的相位分别为和幅度分别为b1和b2;
弹性干扰信号的形式如下
将弹性干扰信号中的进行处理得到弹性干扰状态v1、v2,将弹性干扰信号中的进行处理得到弹性干扰状态v3、v4;所述的将弹性干扰信号中的进行处理得到v1、v2,将弹性干扰信号中的进行处理得到v3、v4的过程如下:
设v2为v1关于时间的一阶导数,则对v2取时间的导数为
同理设v4为v3关于时间的一阶导数,则对v4取时间的导数为
根据上述关系,将其整理成弹性干扰信号状态方程的形式如下
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>3</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>4</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>4</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
将干扰频率ω1和ω2当做状态进行估计;设状态为X=[x v1 v2 v3 v4 ω1 ω2]T,其中x为角速度信号,即x=ω;则角速度和干扰信号的状态方程为
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>3</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>4</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>&omega;</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>&omega;</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>x</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>4</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&omega;</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&omega;</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,az角加速度的测量值;
角速度的测量方程为
z=x+v1+v3+w (4)
式中,z为角速度的测量值;w是零均值高斯白噪声;
测量矩阵为
H=[1 1 0 1 0 0 0] (5)
步骤2、设计状态转移矩阵:
<mrow>
<mi>&Phi;</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<mi>T</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>4</mn>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>5</mn>
</msub>
</mtd>
<mtd>
<mi>T</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>6</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>7</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>5</mn>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&phi;</mi>
<mn>8</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>6</mn>
<mo>)</mo>
</mrow>
</mrow>
式中
<mrow>
<msub>
<mi>&phi;</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mn>1</mn>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<msup>
<mi>T</mi>
<mn>2</mn>
</msup>
<msubsup>
<mi>&omega;</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
φ2=-T2ω1v1 (8)
<mrow>
<msub>
<mi>&phi;</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mo>-</mo>
<msubsup>
<mi>T&omega;</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>9</mn>
<mo>)</mo>
</mrow>
</mrow>
φ4=-T2ω1v2-2Tω1v1 (10)
<mrow>
<msub>
<mi>&phi;</mi>
<mn>5</mn>
</msub>
<mo>=</mo>
<mn>1</mn>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<msup>
<mi>T</mi>
<mn>2</mn>
</msup>
<msubsup>
<mi>&omega;</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
φ6=-T2ω2v3 (12)
<mrow>
<msub>
<mi>&phi;</mi>
<mn>7</mn>
</msub>
<mo>=</mo>
<mo>-</mo>
<msubsup>
<mi>T&omega;</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>13</mn>
<mo>)</mo>
</mrow>
</mrow>
φ8=-T2ω2v4-2Tω2v3 (14)
其中,T为测量周期;
步骤3、设计Kalman滤波方程:
角速度滤波器采用EKF设计,Kalman滤波器方程为
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>Z</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<mi>H</mi>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>21</mn>
<mo>)</mo>
</mrow>
</mrow>
Kk+1=Pk+1,kHT(HPk+1,kHT+R)-1 (22)
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<mi>Q</mi>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>23</mn>
<mo>)</mo>
</mrow>
</mrow>
Pk+1,k+1=(I-Kk+1H)Pk+1,k (24)
式中,是对k+1时刻状态的估计值;是对k+1时刻状态的预报值;Kk和Kk+1分别是k时刻和k+1时刻的滤波器增益;Zk+1为k+1时刻的测量值;Φk+1,k为k时刻到k+1时刻的状态转移矩阵;Pk+1,k为k+1时刻状态预报的协方差矩阵;Pk,k和Pk+1,k+1分别是k时刻和k+1时刻状态估计协方差矩阵;Q为过程噪声矩阵;R为测量噪声矩阵;I为7×7维单位矩阵;
步骤4、根据步骤3设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行滤波处理。
2.根据权利要求1所述的一种双频弹性干扰下角速度滤波方法,其特征在于步骤1所述测量矩阵H通过以下步骤得到:
针对角速度的测量方程有如下形式
z=HX+w
同时,根据角速度的测量方程z=x+v1+v3+w和X=[x v1 v2 v3 v4 ω1 ω2]T,得到H=[11 0 1 0 0 0]。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510828799.8A CN105242058B (zh) | 2015-11-24 | 2015-11-24 | 一种双频弹性干扰下角速度滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510828799.8A CN105242058B (zh) | 2015-11-24 | 2015-11-24 | 一种双频弹性干扰下角速度滤波方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105242058A CN105242058A (zh) | 2016-01-13 |
CN105242058B true CN105242058B (zh) | 2018-03-30 |
Family
ID=55039776
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510828799.8A Active CN105242058B (zh) | 2015-11-24 | 2015-11-24 | 一种双频弹性干扰下角速度滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105242058B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
SU679875A1 (ru) * | 1976-04-06 | 1979-08-15 | Уральский ордена Трудового Красного Знамени политехнический институт им. С.М.Кирова | Способ измерени угловой скорости вращени |
SU815632A1 (ru) * | 1979-05-30 | 1981-03-23 | Институт Проблем Машиностроенияан Украинской Ccp | Устройство дл бесконтактногоизМЕРЕНи чиСлА ОбОРОТОВ |
SU1528148A1 (ru) * | 1982-01-14 | 1990-10-23 | Предприятие П/Я В-8618 | Способ измерени угловой скорости объекта |
FR2917175B1 (fr) * | 2007-06-08 | 2010-04-16 | Eurocopter France | Procede et systeme d'estimation de la vitesse angulaire d'un mobile |
CN102221629A (zh) * | 2011-03-04 | 2011-10-19 | 东南大学 | 一种基于小波变换的车辆横摆角速度滤波测量方法 |
CN102353802A (zh) * | 2011-07-01 | 2012-02-15 | 哈尔滨工程大学 | 一种基于全加速度计的运动载体角速度测量方法 |
CN102435763A (zh) * | 2011-09-16 | 2012-05-02 | 中国人民解放军国防科学技术大学 | 一种基于星敏感器的航天器姿态角速度测量方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0949737A (ja) * | 1995-08-04 | 1997-02-18 | Tamagawa Seiki Co Ltd | 航法信号出力方法 |
-
2015
- 2015-11-24 CN CN201510828799.8A patent/CN105242058B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
SU679875A1 (ru) * | 1976-04-06 | 1979-08-15 | Уральский ордена Трудового Красного Знамени политехнический институт им. С.М.Кирова | Способ измерени угловой скорости вращени |
SU815632A1 (ru) * | 1979-05-30 | 1981-03-23 | Институт Проблем Машиностроенияан Украинской Ccp | Устройство дл бесконтактногоизМЕРЕНи чиСлА ОбОРОТОВ |
SU1528148A1 (ru) * | 1982-01-14 | 1990-10-23 | Предприятие П/Я В-8618 | Способ измерени угловой скорости объекта |
FR2917175B1 (fr) * | 2007-06-08 | 2010-04-16 | Eurocopter France | Procede et systeme d'estimation de la vitesse angulaire d'un mobile |
CN102221629A (zh) * | 2011-03-04 | 2011-10-19 | 东南大学 | 一种基于小波变换的车辆横摆角速度滤波测量方法 |
CN102353802A (zh) * | 2011-07-01 | 2012-02-15 | 哈尔滨工程大学 | 一种基于全加速度计的运动载体角速度测量方法 |
CN102435763A (zh) * | 2011-09-16 | 2012-05-02 | 中国人民解放军国防科学技术大学 | 一种基于星敏感器的航天器姿态角速度测量方法 |
Also Published As
Publication number | Publication date |
---|---|
CN105242058A (zh) | 2016-01-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113670340B (zh) | 基于x/y信号相位差辨识的半球谐振陀螺驻波方位角测量方法及系统 | |
CN104850759B (zh) | 一种风洞强迫振动动稳定性导数试验数据处理方法 | |
CN103090884B (zh) | 基于捷联惯导系统的多普勒计程仪测速误差抑制方法 | |
CN105758404B (zh) | 智能设备的实时定位方法及系统 | |
CN105300381A (zh) | 一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法 | |
CN103823239A (zh) | 频率域优化混合交错网格有限差分正演模拟方法 | |
CN103944174A (zh) | 基于互相关函数滤噪算法的低频振荡在线辨识方法 | |
CN105305920A (zh) | 一种抑制交流伺服系统扭转振动的系统及其方法 | |
CN109960823B (zh) | 风力发电机组的等效风速确定方法和设备 | |
CN103344260A (zh) | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 | |
CN106786561A (zh) | 一种基于自适应卡尔曼滤波的低频振荡模态参数辨识方法 | |
CN109211275A (zh) | 一种陀螺仪的零偏温度补偿方法 | |
CN103115625A (zh) | 一种浮体横纵荡及升沉运动的测量方法及系统 | |
CN104050355B (zh) | 一种基于h∞滤波的机械臂外力估计方法 | |
CN107064559A (zh) | 一种基于角摇摆运动的sins加速度计频率特性测试方法 | |
Wang et al. | Nonlinear multiple integrator and application to aircraft navigation | |
CN112012884A (zh) | 风力发电机组的控制方法及设备 | |
CN106643728A (zh) | 基于自适应频率估计的船舶升沉运动信息估计方法 | |
CN105242058B (zh) | 一种双频弹性干扰下角速度滤波方法 | |
CN107063243A (zh) | 一种基于带限傅里叶线性组合的舰船升沉测量方法 | |
CN102169127A (zh) | 一种具有自适应能力的机抖激光陀螺实时去抖方法 | |
CN104344835B (zh) | 一种基于切换式自适应控制罗经的捷联惯导动基座对准方法 | |
CN103575300B (zh) | 一种基于旋转调制的摇摆基座惯导系统粗对准方法 | |
CN104535082A (zh) | 一种基于飞行试验和理论计算判断惯导元件性能的方法 | |
CN103901459B (zh) | 一种mems/gps组合导航系统中量测滞后的滤波方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |