一种快速自适应的动态惯性导航实时解算去噪方法与流程

1.本发明属于数字去噪技术领域，具体涉及一种快速自适应的动态惯性导航实时解算去噪方法。


背景技术：

2.数字信号的滤波是通过数字滤波器来实现的。数字滤波器是一种用来过滤时间离散信号的数字系统,它是通过对抽样数据进行数学处理来达到频域滤波的目的。最开始通过对信号进行求加权平均来实现对加性噪声的去除，后来通过设计模拟滤波器是现在对其他噪声的去除，随着数字信号的发展，逐渐实用fft变换进行去噪处理，首先经变换得到频谱，通过分析频谱进而知道有用信号与噪声所在的带宽，然后设计对应的数字滤波器实现对信号的去噪处理。虽然低通高通等滤波器能实现去噪，倒是对于随机噪声的处理效果不佳，尤其是对传感器的数据信号处理达不到有效的应用。因此需要提出一种对随机噪声有良好性能的滤波器
3.卡尔曼滤波器在随机序列估计、空间技术、目标跟踪具有广泛的应用，1960年卡尔曼还提出能控性的概念。后人称之为卡尔曼卡尔曼滤波的实质是由量测值重构系统的状态向量。它以“预测—实测—修正”的顺序递推，卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算，卡尔曼滤波的实质是由量测值重构系统的状态向量。它以“预测—实测—修正”的顺序递推，根据系统的量测值来消除随机干扰，再现系统的状态。
4.但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。引起滤波器误差发散的主要原因有两点：
5.(1)描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。
6.(2)由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。


技术实现要素：

7.有鉴于此，本发明的目的在于提供一种快速自适应的动态惯性导航实时解算去噪方法，通过计算相邻两个时刻的差分值，估计此时间段内的信号噪声协方差矩阵，从而不断更新，使得原始信号值逼近理论值。
8.为达到上述目的，本发明提供如下技术方案：
9.本发明一种快速自适应的动态惯性导航实时解算去噪方法，包括以下步骤：
10.步骤1，假设控制小车进行的运动为定速直线运动，小车第k时刻的状态包含位置p
k
和速度v
k
信息[p
k v
k
]’，记为x
k
，经过δt时间，从k
‑
1时刻到k时刻，小车的位置状态的变化写成矩阵的形式即为记状态转换矩阵为a，即
[0011]
x
k
＝ax
k
‑1ꢀꢀ
(1)
[0012]
假设小车的状态位置为x
k
与精确值间满足正态分布，协方差为p
k
，经过x
k
＝ax
k
‑1的变换，k时刻的方差p
k
满足p
k
＝ap
k
‑1a
t
，其中t为时间；
[0013]
步骤2，取协方差矩阵q值为前后时刻差值的平方，公式为
[0014]
p
k
＝ap
k
‑1a
t
+q
ꢀꢀ
(2)
[0015]
q＝(x
k
‑
x
k
‑1)2ꢀꢀ
(3)
[0016]
步骤3，加入一个状态转换的矩阵，把当前的状态向量转化为传感器传回的状态，记矩阵为h
k
；
[0017]
步骤4，联合分布满足方程h
k
x
k
′
＝h
k
x
k
+k(y
k
‑
h
k
x
k
)，k为卡尔曼增益，其值为对以上三式约去左边的h
k
，并进行化简可得
[0018][0019]
x
k
′
＝x
k
+k
′
(y
k
‑
h
k
x
k
)
ꢀꢀ
(5)
[0020]
p
k
′
＝p
k
‑
k
′
h
k
p
k
ꢀꢀ
(6)
[0021]
得到的x
k’与p
k’即可作为k时刻的估计初始状态继续进行下一时刻的预测，传感器的协方差r值采用前后时刻的状态值进行估计，其值估计为
[0022]
r＝((x
k
′‑
x
k
‑1)
×
m)2+nr
ꢀꢀ
(7)
[0023]
其中，m为前后时刻状态对协方差的影响系数，n为惯性系数。
[0024]
进一步，步骤1中，小车的状态位置为x
k
是通过上一时刻和卫星传感器综合得出的一个估计值。
[0025]
进一步，步骤3中，h
k
的取值为[1,0]。
[0026]
本发明的有益效果在于：
[0027]
本发明一种快速自适应的动态惯性导航实时解算去噪方法，在传统卡尔曼滤波的基础上，通过计算相邻两个时刻的差分值，估计此时间段内的信号噪声协方差矩阵，从而不断更新，使得原始信号值逼近理论值，达到理想数据滤波效果。
[0028]
本发明的其他优点、目标和特征将在随后的说明书中进行阐述，并且在某种程度上对本领域技术人员而言是显而易见的，或者本领域技术人员可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
[0029]
为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：
[0030]
图1为传统卡尔曼的滤波效果；
[0031]
图2为卡尔曼滤波状态误差估计；
[0032]
图3为快速自适应卡尔曼滤波效果；
[0033]
图4为自适应卡尔曼滤波状态误差估计。
具体实施方式
[0034]
本发明一种快速自适应的动态惯性导航实时解算去噪方法，其具体步骤如下：
[0035]
1、基本假设，后验概率p(x
k
‑1|y
1,k
‑1)服从高斯分布，假设本传感器系统的动态系统在极小时间间隔内是线性：
[0036]
x
k
＝ax
k
‑1+bu
k
‑1+q
k
‑1[0037]
y
k
＝hx
k
+r
k
[0038]
上式中x
k
是k时刻的系统状态，u
k
是k时刻对系统的控制量。a和b是系统参数，对于多模型系统，他们为矩阵。y
k
是k时刻的测量值，h是测量系统的参数，对于多测量系统，h为矩阵。q
k
和r
k
分别表示过程和测量的噪声。他们被假设成高斯白噪声(whitegaussiannoise)，他们的covariance分别是q，r。
[0039]
2、自适应时间更新与状态更新
[0040]
在算法中系统参数设置如下：
[0041]
a＝1
[0042]
u
k
＝0
[0043]
h＝1
[0044]
则系统方程可简化为：
[0045]
x
k
＝ax
k
‑1+q
k
‑1[0046]
y
k
＝x
k
+r
k
[0047]
为得到系统后一时刻系统状态，则需要利用前一时间的状态，并且经相邻时间间隔内的误差来估计得到此时的协方差矩阵q，而不是与传统的卡尔曼滤波一样使用固定的q。更新规则如下：
[0048]
时间更新
[0049]
x
k
＝ax
k
‑1[0050]
p
k
＝ap
k
‑1a
t
+q
[0051]
状态更新
[0052]
k
k
＝p
k
h
t
(hp
k
h
t
+r)
‑1[0053]
x
k
＝x
k
+k
k
(y
k
‑
hx
k
)
[0054]
p
k
＝(1
‑
k
k
h)p
k
[0055]
r＝((x
k
‑
x
k
‑1)
×
m)2+nr
[0056]
上式中k为增益系数，p为校正系数，与传统卡尔曼不同的是，本算法对r进行了估计，使得每一个的系统估计值都能接近于真实值，采取前后时刻的差值的平方为q的值。
[0057]
3、自适应卡尔曼滤波示例
[0058]
假设我们通过卫星传感器定位一个小车的位置信息，同时可以通过指令给小车提供特定的速度。因为测量误差、地形高程变化、天气状态、地面状态等的缘故，通过卫星传感器定位的小车信息不是完全精准的，通过提供的速度，计算出来的一个时间段后，与上一个
位置的相对位置也不是完全精准的。为了避障等需求，需要综合卫星传感器定位和给小车提供的速度来实现更加准确地定位，从而实现更精确地控制，使用的方法即为卡尔曼滤波。
[0059]
为方便描述，简化模型，假设控制小车进行的运动为定速直线运动。小车第k时刻的状态包含位置p
k
和速度v
k
信息[p
k v
k
]’，记为x
k
。经过δt时间，从k
‑
1时刻到k时刻，小车的位置状态的变化为p
k
＝p
k
‑1+v
k
‑1×
δt,v
k
＝v
k
‑1。写成矩阵的形式即为记状态转换矩阵为a，即
[0060]
x
k
＝ax
k
‑1ꢀꢀ
(1)
[0061]
小车的状态位置x
k
是通过上一时刻和卫星传感器综合得出的一个估计值，不是绝对精准的，我们假设其与精确值间的满足正态分布，协方差为p
k
，根据线性代数知识可知，经过x
k
＝ax
k
‑1的变换，k时刻的方差p
k
满足p
k
＝ap
k
‑1a
t
。
[0062]
除了直接线性变换导致的方差的变化，还有会有环境的不确定性导致的过程噪声的影响q，也即在原协方差的基础上，加上q，即为最终的预测协方差的值。该q值一般取定值，在本文算法中，取其值为前后时刻差值的平方，公式为
[0063]
p
k
＝ap
k
‑1a
t
+q
ꢀꢀ
(2)
[0064]
q＝(x
k
‑
x
k
‑1)2ꢀꢀ
(3)
[0065]
下面加入卫星传感器传回的数据的信息。卫星传感器只传回位置信息，没有速度信息，因此，需要加入一个状态转换的矩阵，把当前的状态向量转化为传感器传回的状态，记矩阵为h
k
，在本实例中，其值为[1,0]，于是，通过估计值得到，传感器的状态可以表示为μ
k
＝h
k
x
k
，协方差可表示为
[0066]
同时，传感器自身的读数值为y
k
，具有协方差值为r。于是得到两个正态分布，(h
k
x
k
,h
k
p
k
h
kt
)，(y
k
,r)。通过这两个正态分布，可以把两个正态分布的密度曲线相乘，得到一个新的正态分布曲线，其为两个正态分布的联合分布，该分布可以做为新一时刻的估计曲线。这个联合正态分布为传感器值的正态分布，与我们要预测的新状态的相差一个转换矩阵h
k
，设新的状态值为x
k’，p
k’，于是，通过对密度分布曲线的数学运算可得，联合分布满足方程h
k
x
k
′
＝h
k
x
k
+k(y
k
‑
h
k
x
k
)，k为卡尔曼增益，其值为对以上三式约去左边的h
k
，并进行化简可得
[0067][0068]
x
k
′
＝x
k
+k
′
(y
k
‑
h
k
x
k
)
ꢀꢀ
(5)
[0069]
p
k
′
＝p
k
‑
k
′
h
k
p
k
ꢀꢀ
(6)
[0070]
得到的x
k’与p
k’即可作为k时刻的估计初始状态继续进行下一时刻的预测。本文中，传感器的协方差r值不取为定值，而是采用前后时刻的状态值进行估计，其值估计为
[0071]
r＝((x
k
′‑
x
k
‑1)
×
m)2+nr
ꢀꢀ
(7)
[0072]
其中，m为前后时刻状态对协方差的影响系数，n为惯性系数。
[0073]
图1为传统卡尔曼的滤波效果，由图可知传统卡尔曼滤波的效果不佳，主要是由于对实时协方差矩阵估计不准确导致的，从图中看出精确值在
‑
0.45附近，而滤波后的结果确大幅振荡。
[0074]
图2为卡尔曼滤波状态误差估计，该图为卡尔曼滤波对状态误差q的估计，可以以看出q的值为一个定值，但是实际上状态误差随着时间时变化的，因此最后的估计值达不到理想效果。
[0075]
图3为快速自适应卡尔曼滤波效果，由图可知快速自适应卡尔曼滤波能更加精确地估计每一个系统状态，及时在外界误差变化没有规律，即误差方差可变的情况下也能有良好的估计性能，因此较传统卡尔曼相比，滤波性能有较大提升；
[0076]
图4为自适应卡尔曼滤波状态误差估计，该图为自适应卡尔曼滤波对状态误差q的估计，可以以看出再第二次迭代以后就误差估计值随时间变化而变化，于实际情况相符，因此最后的估计值达到理想数据滤波效果。
[0077]
最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。