Kalman滤波、扩展Kalman滤波、无迹Kalman滤波和异步滤波的原理及其Matlab代码

2023-12-12 01:12

本文主要是介绍Kalman滤波、扩展Kalman滤波、无迹Kalman滤波和异步滤波的原理及其Matlab代码,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

目录

  • 引言
  • Kalman滤波
    • 代码及其结果展示
  • 扩展Kalman滤波
    • 代码及其结果展示
  • 无迹Kalman滤波
    • 无迹变换
    • 无迹Kalman滤波
    • 代码及其结果展示
  • 异步无迹Kalman滤波
    • 原理
    • 代码及其结果展示

引言

本文给出了Kalman Filter(卡尔曼滤波)、Extended Kalman Filter(扩展卡尔曼滤波)、Unscented Kalman Filter(无迹卡尔曼滤波)和Asynchronous Filter(异步滤波)的原理及其Matlab代码。不同的滤波算法以函数形式在不同的m文件,调用即可使用。

Kalman滤波

该部分展示了kalman滤波的代码及其仿真结果。
状态转移模型
x ( t + 1 ) = A x ( t ) + Γ w ( t ) x(t+1)=Ax(t)+Γw(t) x(t+1)=Ax(t)+Γw(t)
其中 x ( t ) ∈ R n ∗ 1 x(t)∈R^{n*1} x(t)Rn1表示t时刻的状态向量, A ∈ R m ∗ m A∈R^{m*m} ARmm是状态转移矩阵,Γ是噪声系数矩阵,w(t)是t时刻的过程噪声向量, 其为零均值高斯白噪声,协方差阵为Q>0。
量测模型
y i ( t ) = H i x ( t ) + v i ( t ) yi(t)=Hi x(t)+vi(t) yi(t)=Hix(t)+vi(t)
,其中 y i ( t ) yi(t) yi(t)表示t时刻传感器网络中的传感器i的量测向量, H i ∈ R l ∗ m Hi∈R^{l*m} HiRlm是观测矩阵,vi(t)是t时刻的量测噪声向量,其为零均值高斯白噪声,协方差阵为Ri>0。
初始化:
在这里插入图片描述
在这里插入图片描述
状态更新:
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

代码及其结果展示

核心代码:(完整代码见文末)

function [estimate, cov_error_estimate] = kalman_filter(measure, x_last,...P_last,flag)
%kalman_filter 基本kalman滤波算法
%   输入:量测值,上一时刻估计,上一时刻估计误差协方差,是否预测的标志
%   输出:当前时刻估计,当前时刻估计误差协方差
% 当flag为1,则跳过预测步骤
%%
% 状态转移矩阵
global A;
% 系统噪声系数矩阵
global gama;
% 系统噪声协方差
global Q;
% 量测矩阵
global H;
% 量测噪声协方差
global R;
%%
% 如果全零,即无初始化
if ~any(x_last)if measure==0estimate = x_last;cov_error_estimate = P_last;else%   如果是0时刻,则取量测值对应的状态为当前时刻估计,9倍的量测误%差协方差阵为当前时刻估计误差协方差。estimate = H\measure;cov_error_estimate = H\(2*(R))/H';end
elseif flag==1forecast=x_last;P_forecast=P_last;elseforecast = A * x_last;P_forecast = (A*P_last *A'+ gama*Q* gama');endif measure==0estimate=forecast;cov_error_estimate = P_forecast;elsekalman_gain = P_forecast*H'/(H*P_forecast*H'+R);estimate = forecast + kalman_gain*(measure - H*forecast);cov_error_estimate = P_forecast - kalman_gain*H*P_forecast;end
end
end

结果展示:
在这里插入图片描述
在这里插入图片描述

扩展Kalman滤波

扩展卡尔曼滤波是标准卡尔曼滤波在非线性情形下的一种扩展形式,EKF算法是将非线性函数进行泰勒展开,省略高阶项,保留展开项的一阶项,以此来实现非线性函数线性化,最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值,对信号进行滤波。
状态转移方程与量测方程
在这里插入图片描述
泰勒线性化
在这里插入图片描述
在这里插入图片描述
预测步骤
在这里插入图片描述
估计步骤
在这里插入图片描述

代码及其结果展示

核心代码:(完整代码见文末)

function [estimate, cov_error_estimate] = extend_kf(measure, x_last, P_last,...flag,radar)
%kalman_filter 扩展kalman滤波算法
%   输入:量测值,上一时刻估计,上一时刻估计误差协方差,是否预测的标志,
%雷达的状态信息
%   输出:当前时刻估计,当前时刻估计误差协方差
% 当flag为1,则跳过预测步骤
%%
% 状态转移矩阵
global A;
% 系统噪声系数矩阵
global gama;
% 系统噪声协方差
global Q;
% 量测噪声协方差
global R;
%%
% 如果全零,即无初始化
if ~any(x_last)if measure==0estimate = x_last;cov_error_estimate = P_last;else%   如果是0时刻,则取量测值对应的状态为当前时刻估计,9倍的量测误%差协方差阵为当前时刻估计误差协方差。%   此处为9的原因是3倍的标准差为百分之95的可能的误差值,可以认为%包含了最大误差的情况。estimate=[radar(1,1)+measure(1,1)*cos(measure(3,1))*cos(measure(2,1));radar(2,1)+measure(1,1)*sin(measure(3,1));radar(3,1)-measure(1,1)*cos(measure(3,1))*sin(measure(2,1));0;0;0];cov_error_estimate = 99*[1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1;1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1];end
elseif flag==1forecast=x_last;P_forecast=P_last;elseforecast = A * x_last;P_forecast = (A*P_last *A'+ gama*Q* gama');endx=forecast(1,1)-radar(1,1);y=forecast(2,1)-radar(2,1);z=forecast(3,1)-radar(3,1);dist = norm([x;y;z]);dist_horiz = norm([x;z]);H=[x/dist,y/dist,z/dist,0,0,0;z/dist_horiz^2,0,-x/dist_horiz^2,0,0,0;-x*y/(dist^2*dist_horiz),dist_horiz/dist^2,-y*z/(dist_horiz*dist^2),0,0,0];z_est = measure_func([x;y;z]);if measure==0estimate=forecast;cov_error_estimate = P_forecast;elsekalman_gain = P_forecast*H'/(H*P_forecast*H'+R);estimate = forecast + kalman_gain*(measure - z_est);cov_error_estimate = P_forecast - kalman_gain*H*P_forecast;end
end
end

结果展示:
在这里插入图片描述
在这里插入图片描述

无迹Kalman滤波

无迹变换

设存在一个m维随机向量在这里插入图片描述和n维随机向量z,其中z与x为非线性关系
在这里插入图片描述
x的均值为在这里插入图片描述,方差为P;x通过非线性映射得到z。无迹变换就是根据x的统计特性,得到一族点集,该点集称为为σ点;将σ点进行非线性映射可以得的新的点集;通过新的点集得到z的统计特性。一般情况下σ点的数目为2n+1。
无迹变换的具体过程可描述如下:
(1)计算2n+1个点及其权系数
在这里插入图片描述
在这里插入图片描述
其中,在这里插入图片描述决定点的散布程度,通常取正数,在这里插入图片描述通常取为0,β用来代表x的分布情况(正态分布的情况下,最佳值为2),在这里插入图片描述表示矩阵的平方根的第i列,在这里插入图片描述为用于计算均值的权重,在这里插入图片描述为用于计算方差时的权重。
(2)计算σ点通过非线性函数的传播结果
在这里插入图片描述
从而可得
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
此时,我们得到了无迹变换的结果。

无迹Kalman滤波

针对量测为非线性、状态转移为线性的系统模型,无迹Kalman滤波是用无迹变换进行经典卡尔曼滤波中量测方程的映射。
每个递推无迹Kalman滤波的具体步骤如下:
(1)针对线性目标状态转移模型,对于给定的估计和估计误差协方差,基于经典Kalman滤波求状态预测以及预报误差的协方差阵。
在这里插入图片描述
在这里插入图片描述

(2)针对非线性传感器量测模型,用无迹变换求σ点,并通过量测方程的传播。
计算σ点,即
在这里插入图片描述
计算输出的一步提前预测,即
在这里插入图片描述
在获得新的量测后,进行滤波更新,
在这里插入图片描述

代码及其结果展示

核心代码:(完整代码见文末)

function [estimate, cov_error_estimate] = unscented_kf(measure, x_last, P_last,...flag,radar)
%kalman_filter 扩展kalman滤波算法
%   输入:量测值,上一时刻估计,上一时刻估计误差协方差,是否预测的标志,
%雷达的状态信息
%   输出:当前时刻估计,当前时刻估计误差协方差
% 当flag为1,则跳过预测步骤
%%
% 状态转移矩阵
global A;
% 系统噪声系数矩阵
global gama;
% 系统噪声协方差
global Q;
% 量测噪声协方差
global R;
%%
% 如果全零,即无初始化
if ~any(x_last)if measure==0estimate = x_last;cov_error_estimate = P_last;else%   如果是0时刻,则取量测值对应的状态为当前时刻估计,9倍的量测误%差协方差阵为当前时刻估计误差协方差。%   此处为9的原因是3倍的标准差为百分之95的可能的误差值,可以认为%包含了最大误差的情况。% TODO 应该设置为根据实际对象进行变化,这里强制设置为6个状态,%后续对无迹滤波和此处进行改变,让其可以根据实际情况进行随动estimate=[radar(1,1)+measure(1,1)*cos(measure(3,1))*cos(measure(2,1));radar(2,1)+measure(1,1)*sin(measure(3,1));radar(3,1)-measure(1,1)*cos(measure(3,1))*sin(measure(2,1));0;0;0];cov_error_estimate = 1*[1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1;1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1];end
elseif flag==1forecast=x_last;P_forecast=P_last;elseforecast = A * x_last;P_forecast = (A*P_last *A'+ gama*Q* gama');endif measure==0estimate=forecast;cov_error_estimate = P_forecast;else[forecast_point,forecast_weight]=get_point(forecast,P_forecast,size(forecast,1));measure_point = zeros(size(measure,1),size(forecast_point,2));for cou_point = 1:size(forecast_point,2)measure_point(:,cou_point)=measure_func(forecast_point(1:3,cou_point)-radar(1:3,1));endmeasure_expect = sum(forecast_weight(1,:).*measure_point,2);P_z = forecast_weight(2,:).*(measure_point-measure_expect)*...(measure_point-measure_expect)'+R;P_xz = forecast_weight(2,:).*(forecast_point-forecast)*...(measure_point-measure_expect)';kalman_gain = P_xz/P_z;estimate = forecast + kalman_gain*(measure - measure_expect);cov_error_estimate = P_forecast - kalman_gain*P_z*kalman_gain';end
end
end

结果展示:
在这里插入图片描述

异步无迹Kalman滤波

在目标状态估计过程中,量测通常以离散化采样给出,同时带有一个探测时间标志。分布式传感器网络是对多个量测进行处理。由于传感器网络传输存在随机的时间延迟,且各个节点对于量测的预处理时间存在随机性,很可能出现源于某个目标的较晚的量测到达某一节点后较早的量测才到达该节点,这种情况就称为非顺序量测。在目标状态估计过程中,传感器节点通常仅保存航迹的充分统计量。这样,当接收到一个延迟的量测时,假定该量测的时戳为t,为了实时性,估计结果被更新到tz>t时刻,此时需要使用来自时刻t的量测更新tz时刻的估计。这就是在实际的多传感器系统中常见的负时间量测更新问题,原因是t-tz为负,而在标准的滤波问题中总假定为非负。通常情况下的滤波算法不适用于这种情况,因此,就需要对适用于异步滤波的算法进行研究。

原理

首先,我们需要对非线性目标运动模型以不同的时间间隔进行重新的离散化定义。对于任意时间间隔的状态转移矩阵为在这里插入图片描述;任意时间间隔的噪声系数矩阵为在这里插入图片描述;任意时间间隔的系统噪声为
在这里插入图片描述
本文仅研究高斯白噪声随机过程,则根据随机积分的性质可知,离散化后的系统噪声仍为均值为0的高斯白噪声,其协方差矩阵为
在这里插入图片描述
为了便于描述,假定当前时刻为t,而最新的量测时刻为td,且td的量测为t时刻的延迟量测,也就是说
在这里插入图片描述

此时,
在这里插入图片描述
其中,d为td时刻的简化标记。可以得到
在这里插入图片描述
其中在这里插入图片描述在这里插入图片描述的逆矩阵,表示后向状态转矩矩阵。
我们要解决的问题为:在t时刻,已知目标的状态估计和估计误差协方差。同时,我们得到了td时刻的量测。此时,需要用td时刻的量测来更新t时刻状态估计和估计误差协方差,即计算
在这里插入图片描述
其中在这里插入图片描述
在最小均方误差准则下,对于非顺序量测问题的最优更新算法为
在这里插入图片描述
在这里插入图片描述

其中,
在这里插入图片描述

为了求出在这里插入图片描述,我们需要得到Kd和的值。首先
在这里插入图片描述
其中
在这里插入图片描述

由传感器探测模型及最小均方误差准则得
在这里插入图片描述
可知,
在这里插入图片描述
其中
在这里插入图片描述
在这里插入图片描述的σ点为
在这里插入图片描述

根据传感器探测模型,得
在这里插入图片描述
求得计算在这里插入图片描述的σ点为
在这里插入图片描述
同理,根据已知的在这里插入图片描述,求得计算在这里插入图片描述的σ点为
在这里插入图片描述
计算量测的一步提前预测,即
在这里插入图片描述
可知
在这里插入图片描述
此时,因为在这里插入图片描述等于0,可得在这里插入图片描述。由最小均方误差准则可得
在这里插入图片描述
带入式子可求在这里插入图片描述。根据式子可得在这里插入图片描述
在这里插入图片描述
可得在这里插入图片描述的σ点为
在这里插入图片描述
根据非线性变换,可得在这里插入图片描述的σ点为
在这里插入图片描述
在这里插入图片描述的σ点可求
在这里插入图片描述
可得Kd;将Kd和在这里插入图片描述的带入可得在这里插入图片描述。接下来,将在这里插入图片描述在这里插入图片描述带入即可求出在这里插入图片描述。至此,完成了用td时刻的量测来更新t时刻状态估计和估计误差协方差。

代码及其结果展示

核心代码:(完整代码见文末)

function [estimate, cov_error_estimate] = unscented_aysn(measure_aysn,t_asyn,...x_last, P_last,flag,radar,last_est_last,last_P_last,measure)
%kalman_filter 扩展kalman滤波算法
%   输入:量测值,上一时刻估计,上一时刻估计误差协方差,是否预测的标志,
%雷达的状态信息
%   输出:当前时刻估计,当前时刻估计误差协方差
% 当flag为1,则跳过预测步骤
%%
global t_step;
% 状态转移矩阵
global A;
% 系统噪声系数矩阵
global gama;
% 系统噪声协方差
global Q;
% 量测噪声协方差
global R;
%%
last_est_last = A * last_est_last;
last_P_last = (A*last_P_last *A'+ gama*Q* gama');
% 如果全零,即无初始化
if ~any(x_last)if measure_aysn==0estimate = x_last;cov_error_estimate = P_last;else%   如果是0时刻,则取量测值对应的状态为当前时刻估计,9倍的量测误%差协方差阵为当前时刻估计误差协方差。%   此处为9的原因是3倍的标准差为百分之95的可能的误差值,可以认为%包含了最大误差的情况。estimate=[radar(1,1)+measure_aysn(1,1)*cos(measure_aysn(3,1))*cos(measure_aysn(2,1));radar(2,1)+measure_aysn(1,1)*sin(measure_aysn(3,1));radar(3,1)-measure_aysn(1,1)*cos(measure_aysn(3,1))*sin(measure_aysn(2,1));0;0;0];cov_error_estimate = 1*[1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1;1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1];end
elseif flag==1forecast=x_last;P_forecast=P_last;elseforecast = A * x_last;P_forecast = (A*P_last *A'+ gama*Q* gama');endif measure_aysn==0estimate=forecast;cov_error_estimate = P_forecast;elsenum_state = size(forecast,1);Q_td_t =  Q_t1_t2(t_asyn,t_step);[w_td2t_t_1_point,~] = ...get_point(zeros(num_state,1), Q_td_t,3);[last_est_point,last_est_weight] = ...get_point(last_est_last, last_P_last, 3);measure_t_1_point = zeros(size(measure,1),size(last_est_point,2));for cou_point = 1:size(last_est_point,2)measure_t_1_point(:,cou_point)=measure_func(...last_est_point(1:3,cou_point)-radar(1:3,1));endmeasure_expect_t_1 = sum(last_est_weight(1,:).*measure_t_1_point,2);cov_zt_t_1 = last_est_weight(2,:).*(measure_t_1_point-...measure_expect_t_1)*(measure_t_1_point-measure_expect_t_1)'+R;% TODO 应该修正R后的pointcov_w_td2t_zt_t_1= last_est_weight(2,:).*(w_td2t_t_1_point)*...(measure_t_1_point-measure_expect_t_1)';w_td2t = cov_w_td2t_zt_t_1/cov_zt_t_1*(measure-...measure_expect_t_1);cov_w_td2t_t = Q_td_t-cov_w_td2t_zt_t_1/cov_zt_t_1*...cov_w_td2t_zt_t_1';[forecast_point,forecast_weight]=get_point(forecast,P_forecast,3);% (3-25)backcast = A_t1_t2(t_step,t_asyn)*(forecast-w_td2t);P_backcast = A_t1_t2(t_step,t_asyn)*P_forecast*...(A_t1_t2(t_step,t_asyn))'-A_t1_t2(t_step,t_asyn)*...(cov_w_td2t_t)*(A_t1_t2(t_step,t_asyn))';[backcast_point,backcast_weight]=get_point(backcast,P_backcast,3);measure_expect_t_point = zeros(size(measure,1),size(backcast_point,2));for cou_point = 1:size(backcast_point,2)measure_expect_t_point(:,cou_point)=measure_func(...backcast_point(1:3,cou_point)-radar(1:3,1));endmeasure_expect_t = sum(backcast_weight(1,:).*measure_expect_t_point,2);% (3-36)P_z = backcast_weight(2,:).*(measure_expect_t_point-...measure_expect_t)*(measure_expect_t_point-measure_expect_t)'+R;% (3-37)         % TODO 应该修正R后的pointP_xz = forecast_weight(2,:).*(forecast_point-forecast)*...(measure_expect_t_point-measure_expect_t)';kalman_gain = P_xz/P_z;estimate = forecast + kalman_gain*(measure_aysn - measure_expect_t);cov_error_estimate = P_forecast - P_xz/P_z*P_xz';end
end
end

结果展示:
在这里插入图片描述
在这里插入图片描述
完整代码分为多个文件,数量较多,无法放在文章中。完整代码在公众号(沸腾的火锅资源号)中自取。

这篇关于Kalman滤波、扩展Kalman滤波、无迹Kalman滤波和异步滤波的原理及其Matlab代码的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/482720

相关文章

利用Python调试串口的示例代码

《利用Python调试串口的示例代码》在嵌入式开发、物联网设备调试过程中,串口通信是最基础的调试手段本文将带你用Python+ttkbootstrap打造一款高颜值、多功能的串口调试助手,需要的可以了... 目录概述:为什么需要专业的串口调试工具项目架构设计1.1 技术栈选型1.2 关键类说明1.3 线程模

Python Transformers库(NLP处理库)案例代码讲解

《PythonTransformers库(NLP处理库)案例代码讲解》本文介绍transformers库的全面讲解,包含基础知识、高级用法、案例代码及学习路径,内容经过组织,适合不同阶段的学习者,对... 目录一、基础知识1. Transformers 库简介2. 安装与环境配置3. 快速上手示例二、核心模

Java的栈与队列实现代码解析

《Java的栈与队列实现代码解析》栈是常见的线性数据结构,栈的特点是以先进后出的形式,后进先出,先进后出,分为栈底和栈顶,栈应用于内存的分配,表达式求值,存储临时的数据和方法的调用等,本文给大家介绍J... 目录栈的概念(Stack)栈的实现代码队列(Queue)模拟实现队列(双链表实现)循环队列(循环数组

Redis消息队列实现异步秒杀功能

《Redis消息队列实现异步秒杀功能》在高并发场景下,为了提高秒杀业务的性能,可将部分工作交给Redis处理,并通过异步方式执行,Redis提供了多种数据结构来实现消息队列,总结三种,本文详细介绍Re... 目录1 Redis消息队列1.1 List 结构1.2 Pub/Sub 模式1.3 Stream 结

使用Python实现一个优雅的异步定时器

《使用Python实现一个优雅的异步定时器》在Python中实现定时器功能是一个常见需求,尤其是在需要周期性执行任务的场景下,本文给大家介绍了基于asyncio和threading模块,可扩展的异步定... 目录需求背景代码1. 单例事件循环的实现2. 事件循环的运行与关闭3. 定时器核心逻辑4. 启动与停

使用Java将DOCX文档解析为Markdown文档的代码实现

《使用Java将DOCX文档解析为Markdown文档的代码实现》在现代文档处理中,Markdown(MD)因其简洁的语法和良好的可读性,逐渐成为开发者、技术写作者和内容创作者的首选格式,然而,许多文... 目录引言1. 工具和库介绍2. 安装依赖库3. 使用Apache POI解析DOCX文档4. 将解析

C++使用printf语句实现进制转换的示例代码

《C++使用printf语句实现进制转换的示例代码》在C语言中,printf函数可以直接实现部分进制转换功能,通过格式说明符(formatspecifier)快速输出不同进制的数值,下面给大家分享C+... 目录一、printf 原生支持的进制转换1. 十进制、八进制、十六进制转换2. 显示进制前缀3. 指

Spring Boot循环依赖原理、解决方案与最佳实践(全解析)

《SpringBoot循环依赖原理、解决方案与最佳实践(全解析)》循环依赖指两个或多个Bean相互直接或间接引用,形成闭环依赖关系,:本文主要介绍SpringBoot循环依赖原理、解决方案与最... 目录一、循环依赖的本质与危害1.1 什么是循环依赖?1.2 核心危害二、Spring的三级缓存机制2.1 三

C#中async await异步关键字用法和异步的底层原理全解析

《C#中asyncawait异步关键字用法和异步的底层原理全解析》:本文主要介绍C#中asyncawait异步关键字用法和异步的底层原理全解析,本文给大家介绍的非常详细,对大家的学习或工作具有一... 目录C#异步编程一、异步编程基础二、异步方法的工作原理三、代码示例四、编译后的底层实现五、总结C#异步编程

使用Python实现全能手机虚拟键盘的示例代码

《使用Python实现全能手机虚拟键盘的示例代码》在数字化办公时代,你是否遇到过这样的场景:会议室投影电脑突然键盘失灵、躺在沙发上想远程控制书房电脑、或者需要给长辈远程协助操作?今天我要分享的Pyth... 目录一、项目概述:不止于键盘的远程控制方案1.1 创新价值1.2 技术栈全景二、需求实现步骤一、需求