CN109902568A - 一种自适应卡尔曼滤波算法 - Google Patents
一种自适应卡尔曼滤波算法 Download PDFInfo
- Publication number
- CN109902568A CN109902568A CN201910056700.5A CN201910056700A CN109902568A CN 109902568 A CN109902568 A CN 109902568A CN 201910056700 A CN201910056700 A CN 201910056700A CN 109902568 A CN109902568 A CN 109902568A
- Authority
- CN
- China
- Prior art keywords
- matrix
- state
- moment
- covariance
- correction factor
- 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.)
- Pending
Links
Landscapes
- Feedback Control In General (AREA)
Abstract
本发明公开了一种自适应卡尔曼滤波算法,用于去除噪声。本发明提供的方法包括:建立滤波系统的状态向量模型和测量向量模型;系统初始化后,将卡尔曼滤波预测值作为样本回归拟合值,计算拟合优度确定系数和修正系数;根据所述修正系数,修正过程噪声协方差矩阵;根据所述状态向量模型和所述测量向量模型中的传输矩阵、测量矩阵,以及修正后的过程噪声协方差矩阵,计算卡尔曼增益,以及计算状态估计值和估计误差协方差。通过本发明提供的技术方案,可以实现实时、高效的进行滤波,并保证滤波精度。
Description
技术领域
本发明涉及信号处理领域,尤其涉及一种自适应卡尔曼滤波算法。
背景技术
卡尔曼滤波是采用信号与噪声的状态空间模型来描述系统,以最小均方差误差为估计原则的一套递推估算算法,由于能够实时去除噪声还原真实数据,目前已得到广泛的使用。
然而,当前常使用的卡尔曼滤波算法,只能依靠系统数学模型的准确及噪声统计特征的完整才能保证精度,而实际应用中,对所研究工程对象的数学模型了解不透以及动态过程中内外因素的影响,很多系统的数学模型和噪声统计特性是未知的或近似已知的,使用不准确的数学模型和噪声统计特性通常会导致传统卡尔曼滤波精度降低,甚至发散,难以保证滤波精度。
发明内容
本发明实施例提供了一种自适应卡尔曼滤波算法,能够实现准确高效的滤波,保证滤波精度。
在本发明实施例的第一方面,提供了一种数字滤波方法,该方法包括:
建立滤波系统的状态向量模型和测量向量模型;
系统初始化后,将卡尔曼滤波预测值作为样本回归拟合值,计算拟合优度确定系数和修正系数;
根据所述修正系数,修正过程噪声协方差矩阵;
根据所述状态向量模型和所述测量向量模型中的传输矩阵、测量矩阵,以及修正后的过程噪声协方差矩阵,计算卡尔曼增益,以及计算状态估计值和估计误差协方差。
从以上技术方案可以看出,本发明实施例具有以下优点:
在本发明实施例中,通过将卡尔曼滤波的预测值作为样本回归拟合值计算拟合优度确定系数,并计算修正系数,进而根据修正系数可以自动的修正过程噪声协方差矩阵,从而保证卡尔曼增益及状态估计、估计误差协方差的准确计算。本申请提供的计算方案,既能保证滤波系统的滤波精度,也具有较快的响应速度,可以实现实时、高效的滤波,且是一种简单易行的滤波算法。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种自适应卡尔曼滤波算法的一个实施例流程图;
具体实施方式
本发明实施例提供了一种自适应卡尔曼滤波算法,用于实时、准确的进行滤波,减少噪声干扰。
为使得本发明的发明目的、特征、优点能够更加的明显和易懂,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,下面所描述的实施例仅仅是本发明一部分实施例,而非全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
请参阅图1,本发明实施例中提供的一种卡尔曼滤波算法的一个实施例流程图包括:
S101、建立滤波系统的状态向量模型和测量向量模型;
可选的,所述状态向量模型和测量向量模型分别为:
xk=Ak-1xk-1+wk-1
zk=Hkxk+vk
其中,xk为k时刻的状态向量;zk为k时刻的测量向量;Ak-1为k-1时刻到k时刻的状态转移矩阵,即传输矩阵;Hk为k时刻的测量矩阵;wk和vk分别表示k时刻的过程噪声序列和测量噪声序列。
优选的,当过程噪声wk和测量噪声vk为不相关的零均值高斯白噪声,则wk和vk满足:
E[wk]=0
E[vk]=0
其中,为Kronecker-δ函数。
S102、系统初始化后,将卡尔曼滤波预测值作为样本回归拟合值,计算拟合优度确定系数和修正系数;
可选的,所述系统初始化具体为:
其中,x0为状态向量初始值;为后验状态估计初始值;为后验状态估计协方差初始值;为测量数据平均值初始值,RSS0为残差平方和初始值;TSS0为总平方和初始值。
进一步的,根据公式(1)和公式(2)计算所述拟合优度确定系数和所述修正系数:
其中,R2为拟合优度确定系数,αk为修正系数,TSSk为总平方和,ESSk为回归平方和,RSSk为残差平方和;
需要注意的是,R2的取值范围通常被限定到[0,1],所以αk的取值范围为[0,1],实际应用中可以给αk设定一个下限值αmin以防止过程噪声矩阵过小导致滤波结果过度平滑。
进而得到TSSk,ESSk和RSSk的递推算式为:
其中,为从开始时刻到k时刻k个测量数据的平均值;zk为k时刻的实际采集数据;Hk为k时刻的测量矩阵;为在已知第k步以前状态情况下第k步的先验状态估计。
S103、根据所述修正系数,修正过程噪声协方差矩阵;
可选的,根据公式(3)计算修正后的过程协方差矩阵:
Qk=αk Q0 (3)
其中,αk为修正系数,Q0为初始过程协方差矩阵,Qk为修正后的协方差矩阵。
S104、根据状态向量模型和测量向量模型中的传输矩阵、测量矩阵,以及修正后的过程噪声协方差矩阵,计算卡尔曼增益、状态估计值和估计误差协方差。
具体的,根据公式(4)和公式(5),计算先验状态估计和估计误差协方差:
其中,为在已知第k步以前状态情况下第k步的先验状态估计;为在已知测量变量zk-1时第k-1步的后验状态估计;Ak-1为k-1时刻到k时刻的状态转移矩阵,即传输矩阵,是先验估计误差协方差,为后验估计误差协方差,是Ak-1的转置矩阵,Qk是第k步的过程噪声协方差矩阵;
根据公式(6)计算卡尔曼增益:
其中Kk为卡尔曼增益,为k时刻测量矩阵Hk的转置矩阵,为已知第k步以前状态下第k步的先验估计误差协方差,Rk是第k步的测量噪声协方差;
根据公式(7)和公式(8)计算状态估计和误差协方差:
其中,为后验状态估计,为后验估计误差协方差。
在上述算法中,根据实际工程的基本情况构造系统的粗略模型,再通过拟合优度对系统状态模型进行评估,得到拟合优度确定系数,最后求取过程噪声协方差的修正系数,修正过程噪声协方差,实现过程噪声协方差的自适应调整,进而保证系统的滤波精度。
应理解,上述实施例中各步骤的序号的大小并不意味着执行顺序的先后,各过程的执行顺序应以其功能和内在逻辑确定,而不应对本发明实施例的实施过程构成任何限定。
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的系统,装置和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
在上述实施例中,对各个实施例的描述都各有侧重,某个实施例中没有详述或记载的部分,可以参见其它实施例的相关描述。
以上所述,以上实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (6)
1.一种自适应卡尔曼滤波算法,其特征在于,包括:
建立滤波系统的状态向量模型和测量向量模型;
系统初始化后,将卡尔曼滤波预测值作为样本回归拟合值,计算拟合优度确定系数和修正系数;
根据所述修正系数,修正过程噪声协方差矩阵;
根据所述状态向量模型和所述测量向量模型中的传输矩阵、测量矩阵,以及修正后的过程噪声协方差矩阵,计算卡尔曼增益,以及计算状态估计值和估计误差协方差。
2.如权利要求1所述的方法,其特征在于,所述状态向量模型和测量向量模型分别为:
xk=Ak-1xk-1+wk-1
zk=Hkxk+vk
其中,xk为k时刻的状态向量;zk为k时刻的测量向量;Ak-1为k-1时刻到k时刻的状态转移矩阵,即传输矩阵;Hk为k时刻的测量矩阵;wk和vk分别表示k时刻的过程噪声序列和测量噪声序列。
3.如权利要求1所述的方法,其特征在于,所述系统初始化具体为:
TSS0=0
其中,x0为状态向量初始值;为后验状态估计初始值;为后验状态估计协方差初始值;为测量数据平均值初始值,RSS0为残差平方和初始值;TSS0为总平方和初始值。
4.如权利要求1所述的方法,其特征在于,所述将卡尔曼滤波预测值作为样本回归拟合值,计算拟合优度确定系数和修正系数具体为:
根据公式(1)和公式(2)计算所述拟合优度确定系数和所述修正系数:
其中,R2为拟合优度确定系数,αk为修正系数,TSSk为总平方和,ESSk为回归平方和,RSSk为残差平方和;
其中,为从开始时刻到k时刻k个测量数据的平均值;zk为k时刻的实际采集数据;Hk为k时刻的测量矩阵;为在已知第k步以前状态情况下第k步的先验状态估计。
5.如权利要求1所述的方法,其特征在于,所述根据所述修正系数,修正过程噪声协方差矩阵具体为:
根据公式(3)计算修正后的过程协方差矩阵:
Qk=αk Q0 (3)
其中,αk为修正系数,Q0为初始过程协方差矩阵,Qk为修正后的协方差矩阵。
6.如权利要求1所述的方法,其特征在于,所述根据所述状态向量模型和所述测量向量模型中的传输矩阵、测量矩阵,以及修正后的过程噪声协方差矩阵,计算卡尔曼增益,以及计算状态估计值和估计误差协方差具体为:
根据公式(4)和公式(5),计算先验状态估计和估计误差协方差:
其中,为在已知第k步以前状态情况下第k步的先验状态估计;为在已知测量变量zk-1时第k-1步的后验状态估计;是先验估计误差协方差,是Ak-1的转置矩阵,Qk是第k步的过程噪声协方差矩阵;
根据公式(6)计算卡尔曼增益:
其中Kk为卡尔曼增益,表示k时刻测量矩阵Hk的转置矩阵,为已知第k步以前状态下第k步的先验估计误差协方差,Rk是第k步的测量噪声协方差;
根据公式(7)和公式(8)计算状态估计和误差协方差:
其中,为后验状态估计,为后验估计误差协方差。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910056700.5A CN109902568A (zh) | 2019-01-22 | 2019-01-22 | 一种自适应卡尔曼滤波算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910056700.5A CN109902568A (zh) | 2019-01-22 | 2019-01-22 | 一种自适应卡尔曼滤波算法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109902568A true CN109902568A (zh) | 2019-06-18 |
Family
ID=66943919
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910056700.5A Pending CN109902568A (zh) | 2019-01-22 | 2019-01-22 | 一种自适应卡尔曼滤波算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109902568A (zh) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110569410A (zh) * | 2019-08-30 | 2019-12-13 | 广西师范大学 | 一种测距数据处理方法、装置及计算机可读存储介质 |
CN110609973A (zh) * | 2019-08-27 | 2019-12-24 | 广东艾科技术股份有限公司 | 一种用于流量测量的卡尔曼滤波方法 |
CN110674784A (zh) * | 2019-09-29 | 2020-01-10 | 润电能源科学技术有限公司 | 电网频率滤波方法、用户设备、存储介质及装置 |
CN111666891A (zh) * | 2020-06-08 | 2020-09-15 | 北京百度网讯科技有限公司 | 用于估计障碍物运动状态的方法和装置 |
CN111881955A (zh) * | 2020-07-15 | 2020-11-03 | 北京经纬恒润科技有限公司 | 多源传感器信息融合方法及装置 |
CN112114255A (zh) * | 2020-09-18 | 2020-12-22 | 中国计量大学 | 一种基于联合集合卡尔曼滤波的锂电池荷电状态估计方法 |
CN112230801A (zh) * | 2020-10-23 | 2021-01-15 | 上海磐启微电子有限公司 | 应用于触摸轨迹的卡尔曼平滑处理方法、存储器及设备 |
CN112269192A (zh) * | 2020-10-22 | 2021-01-26 | 云南航天工程物探检测股份有限公司 | 一种快速自适应的动态北斗监测实时解算去噪方法 |
CN112660144A (zh) * | 2020-12-04 | 2021-04-16 | 上汽通用五菱汽车股份有限公司 | 横摆角速度滤波方法、控制终端、车辆及存储介质 |
CN113422593A (zh) * | 2021-07-05 | 2021-09-21 | 北京信息科技大学 | 滤波方法、滤波器、计算机可读存储介质、处理器和fpga |
CN114137032A (zh) * | 2021-09-07 | 2022-03-04 | 北京联合大学 | 一种大动态范围砂岩模型电阻率测量装置及测量方法 |
CN115114578A (zh) * | 2021-03-19 | 2022-09-27 | Aptiv技术有限公司 | 确定对象传感器的噪声统计的方法 |
-
2019
- 2019-01-22 CN CN201910056700.5A patent/CN109902568A/zh active Pending
Cited By (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110609973B (zh) * | 2019-08-27 | 2023-09-29 | 广东艾科技术股份有限公司 | 一种用于流量测量的卡尔曼滤波方法 |
CN110609973A (zh) * | 2019-08-27 | 2019-12-24 | 广东艾科技术股份有限公司 | 一种用于流量测量的卡尔曼滤波方法 |
CN110569410A (zh) * | 2019-08-30 | 2019-12-13 | 广西师范大学 | 一种测距数据处理方法、装置及计算机可读存储介质 |
CN110674784A (zh) * | 2019-09-29 | 2020-01-10 | 润电能源科学技术有限公司 | 电网频率滤波方法、用户设备、存储介质及装置 |
CN111666891A (zh) * | 2020-06-08 | 2020-09-15 | 北京百度网讯科技有限公司 | 用于估计障碍物运动状态的方法和装置 |
CN111666891B (zh) * | 2020-06-08 | 2023-09-26 | 北京百度网讯科技有限公司 | 用于估计障碍物运动状态的方法和装置 |
CN111881955A (zh) * | 2020-07-15 | 2020-11-03 | 北京经纬恒润科技有限公司 | 多源传感器信息融合方法及装置 |
CN111881955B (zh) * | 2020-07-15 | 2023-07-04 | 北京经纬恒润科技股份有限公司 | 多源传感器信息融合方法及装置 |
CN112114255A (zh) * | 2020-09-18 | 2020-12-22 | 中国计量大学 | 一种基于联合集合卡尔曼滤波的锂电池荷电状态估计方法 |
CN112114255B (zh) * | 2020-09-18 | 2023-02-28 | 中国计量大学 | 一种基于联合集合卡尔曼滤波的锂电池荷电状态估计方法 |
CN112269192B (zh) * | 2020-10-22 | 2024-02-02 | 云南航天工程物探检测股份有限公司 | 一种快速自适应的动态北斗监测实时解算去噪方法 |
CN112269192A (zh) * | 2020-10-22 | 2021-01-26 | 云南航天工程物探检测股份有限公司 | 一种快速自适应的动态北斗监测实时解算去噪方法 |
CN112230801A (zh) * | 2020-10-23 | 2021-01-15 | 上海磐启微电子有限公司 | 应用于触摸轨迹的卡尔曼平滑处理方法、存储器及设备 |
CN112660144A (zh) * | 2020-12-04 | 2021-04-16 | 上汽通用五菱汽车股份有限公司 | 横摆角速度滤波方法、控制终端、车辆及存储介质 |
CN112660144B (zh) * | 2020-12-04 | 2022-06-24 | 上汽通用五菱汽车股份有限公司 | 横摆角速度滤波方法、控制终端、车辆及存储介质 |
CN115114578A (zh) * | 2021-03-19 | 2022-09-27 | Aptiv技术有限公司 | 确定对象传感器的噪声统计的方法 |
CN113422593A (zh) * | 2021-07-05 | 2021-09-21 | 北京信息科技大学 | 滤波方法、滤波器、计算机可读存储介质、处理器和fpga |
CN113422593B (zh) * | 2021-07-05 | 2024-04-26 | 北京信息科技大学 | 滤波方法、滤波器、计算机可读存储介质、处理器和fpga |
CN114137032A (zh) * | 2021-09-07 | 2022-03-04 | 北京联合大学 | 一种大动态范围砂岩模型电阻率测量装置及测量方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109902568A (zh) | 一种自适应卡尔曼滤波算法 | |
CN103927436A (zh) | 一种自适应高阶容积卡尔曼滤波方法 | |
CN102968552A (zh) | 一种卫星轨道数据预估与修正方法 | |
CN108801131B (zh) | 北斗高频变形监测数据的滤波方法及系统 | |
CN111006691B (zh) | 传感器校漂方法、装置、可读存储介质、电子设备及无人机 | |
JP2014215822A (ja) | 状態推定装置、方法、及びプログラム | |
CN107504974B (zh) | 地形分块与地形测点加权的地形匹配定位方法 | |
CN108509379A (zh) | 全局估计自适应两阶段平方根容积滤波的方法 | |
CN111077550A (zh) | 一种应用于智能终端rtd定位的粗差探测方法及系统 | |
CN108648154B (zh) | 相位图的滤波评价方法 | |
CN105115573A (zh) | 一种洪水流量预报的校正方法和装置 | |
CN108446256A (zh) | 局部估计自适应两阶段平方根容积滤波的方法 | |
CN106896363A (zh) | 一种水下目标主动跟踪航迹起始方法 | |
CN112051569A (zh) | 雷达目标跟踪速度修正方法及装置 | |
CN109618288B (zh) | 基于深度卷积神经网络的无线传感网距离测量系统及方法 | |
CN110850450A (zh) | 一种卫星钟差参数的自适应估计方法 | |
Brinkman et al. | Advanced gaussian filters | |
CN103971005A (zh) | 一种靠近湖泊岸边的污染源定位方法 | |
CN104407366B (zh) | 一种对伪距进行平滑处理的方法 | |
CN104331087B (zh) | 一种鲁棒的水下传感器网络目标跟踪方法 | |
CN109507696A (zh) | 一种基于自适应滤波的gnss伪距多路径消除方法 | |
CN108445517B (zh) | 一种定位信号滤波方法、装置及设备 | |
CN115047505B (zh) | 基于载波相位差分辅助的gnss定位方法及导航方法 | |
CN109117965B (zh) | 基于卡尔曼滤波器的系统状态预测装置和方法 | |
CN115713008A (zh) | 流域洪水模拟计算的实时校正方法、系统、装置及介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190618 |