线性卡尔曼滤波原理及公式推导
目录
1. 卡尔曼滤波绪论
Kalman滤波是一种时域滤波方法,采用状态空间方法描述系统,算法采用递推形式,数据存储量小,不仅可以处理平稳随机过程,也可以处理多维和非平稳随机过程。
1.1 滤波基础知识
-
基本概念
滤波 | 滤波一词起源于通信理论,它是从含有干扰的接收信号中提取有用信号的一种技术。而更广泛地,滤波是指利用一定的手段抑制无用信号,增强有用信号的数字信号处理过程。 |
噪声 | 无用信号,也叫噪声,是指观测数据对系统没有贡献或者起干扰作用的信号。在通信中,无用信号表现为特定波段频率、杂波;在传感器数据测量中,无用信号表现为幅度干扰。 |
状态空间表达式 | 状态方程、观测方程、状态转移矩阵、观测矩阵 |
1.2 Kalman滤波的背景
Kalman在20世纪60年代初提出了Kalman滤波方法。Kalman滤波方法是一种时域方法。它把状态空间的概念引入随机估计理论中,把信号过程视为白噪声作用下的一个线性系统的输出,用状态方程来描述这种输入-输出关系,估计过程中利用系统状态方程、观测方程和白噪声激励,即系统过程噪声和观测噪声,它们的统计特性形成滤波算法。由于所用的信息都是时域内的量,所以Kalman滤波不但可以对平稳的一维随机过程进行估计,也可以对非平稳的、多维随机过程进行估计。 为了实现对动态系统的控制,首先需要了解被控对象的实时状态。对于复杂动态系统应用,通常无法测量每一个需要控制的变量,而Kalman滤波能够利用这些有限的、不直接的、包含噪声的测量信息去估计那些缺失的信息。此外,Kalman滤波也被用于预测动态系统未来的变化趋势。
1.3 Kalman滤波的发展
-
估计方法
-
最小二乘
1795年德国约翰-弗里德里希-高斯提出。最小二乘法没有考虑被蛊常数和观测数据的统计特性,因此,这种方法不是最优估计,但是由于最小二乘法在计算上比较简单,使得成为一种广泛应用的估计方法。
-
在极大似然估计
1912年英国统计与遗传学家罗纳德·费希尔(Ronald Fisher,1890—1962年)提出了极大似然估计方法,从概率密度出发来考虑估计问题,对估计理论做出了重大贡献。
-
Wiener滤波
对于随机过程的估计,到20世纪30年代才积极发展起来。1940年,控制论的创始人之一——美国学者诺伯特·维纳(Norbert Wiener,1894—1964年)根据火力控制上的需要提出一种在频域中设计统计最优滤波器的方法,该方法被称为Wiener滤波。
-
Kalman滤波
1960年,卡尔曼提出了离散系统Kalman滤波;1961年,他又与布西(R.S.Bucy)合作,把这一滤波方法推广到连续时间系统中去,从而形成Kalman滤波设计理论。 Bucy等人致力研究Kalman滤波理论在非线性系统和非线性观测下的扩展Kalman滤波,扩展了Kalman滤波的适用范围。扩展Kalman滤波(Extended Kalman Filter,EKF)是一种应用广泛的非线性系统滤波方法。这种滤波器的思想是将非线性系统一阶线性化,然后利用标准Kalman滤波,其存在的问题是线性化过程会带来近似误差。 1999年,S.Julier提出无迹Kalman滤波,它是以UT变换为基础,采用Kalman线性滤波的框架,摒弃了对非线性函数进行线性化的传统做法。对于一步预测方程,使用UT变换来处理均值和协方差的非线性传递,就成为UKF算法。UKF无需像EKF那样要计算Jacobian矩阵,无需忽略高阶项,因而计算精度较高。
-
1.4 Kalman滤波的应用
-
导航制导、目标定位和跟踪领域
-
通信与信号处理、数字图像处理、语音信号处理
-
天气预报、地震预报
-
地质勘探、矿物开采
-
故障诊断、检测
-
证券股票市场预测
2. 线性Kalman滤波
2.1 射影定理
-
由y对x的线性估计:
极小化性能指标:
随机变量的线性最小方差估计:
-
射影:
,称
与y不相关为
与y垂直(正交),并称
为x在y上的射影。摄影的几何意义图1所示。
图1 射影的几何意义
-
流线型射影
设随机序列存在二阶矩,有
-
新息
定义新息为, 新息几何意义如图2所示。规定
。
则得到信息序列表达式
图2 新息的几何意义
-
y的最优预报值为
-
推论:
2.2 递推射影定理
设随机序列存在二阶矩
递推射影定理:
2.3 Kalman滤波器
-
动态系统状态方程
k为离散时间,系统在k时刻的状态X(k);Y(k)为对应状态的观测信号;W(k)为输入白噪声:V(k)为观测噪声。得到状态方程。
-
线性卡曼滤波假设
-
假设一
W(k),V(k)是均值为零、方差阵各位Q和R的不相关白噪声。
-
假设二
初始状态X(0)不相关与W(k)和V(k)
-
-
递推kalman filter
-
状态一步预测
-
-
状态更新
-
-
录波增益矩阵
-
-
一步预测协方差阵
-
-
协方差阵更新
-
-
-
卡曼滤波的参数处理
-
噪声矩阵处理
假设W(k)和V(k)是高斯白噪声,方差分别为Q何R。 在实际应用中要确定Q和R比较难,通茶通过传感器参数来获取。
-
特殊情况的处理
A.H不确定。建好数学模型,调整录波器争议,这种情况下,一般使用自适应滤波。 含有在控制量的系统描述。有U。要将BU(k)加入到预测中,增益矩阵和误差矩阵的递推方式完全一致。 系统噪声为有色噪声。写成增广矩阵。
-
含有控制量的系统描述
-
-
卡曼滤波算法的特点
-
适用于平稳非平稳随机过程
-
时域滤波算法
-
预测修正
-
实时在线计算量少
-
-
线性卡曼滤波的应用案例
-
卡曼瑞博在温度测量中的应用
-
状态方程
-
-
-
Matlab仿真
-
-
-
%%%%%%%%%
%功能说明: Kalman滤波用在一维温度数据测量 系统中
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N= 120;%采样点的个数,时间单位是分钟,可理解为试验进行了60分钟的测量
CON=25;%室内温度的理论值,在这个理论值得基础上受过程噪声会有波动
%对状态和测量初始化
Xexpect=CON*ones(1,N); %期望的温度是恒定的25°C,但真实温度不可能会这样的
X=zeros(1,N); %房间各时刻真实温度值
Xkf=zeros(1,N); % Kalman滤波处理的状态,也叫估计值
Z= zeros(1,N); %温度计测量值
P=zeros(1,N);
%赋初值
X(1)=25.1; %假如初始值房间温度为25.1°C
P(1)=0.01; %初始值的协方差
Z(1)=24.9;
Xkf(1)=Z(1); %初始测量值为24.9°C,可以作为滤波器的初始估计状态
%噪声
Q=0.01;
R=0.25;
W=sqrt(Q)*randn(1,N); %方差决定噪声的大小
V=sqrt(R)*randn(1,N); %方差决定噪声的大小
%系统矩阵
F=1;
G=1;
H=1;
I=eye(1); %本系统状态为一-维
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%
%模拟房间温度和测量过程,并滤波
for k=2:N
%第一步:随时间推移,房间真实温度波动变化
% k时刻房间的真实温度,对于温度计来说,这个真实值是不知道的,但是它的
%存在又是客观事实,读者要深刻领悟这个计算机模拟过程
X(k)=F*X(k-1)+G*W(k-1);
%第二步:随时间推移,获取实时数据
%温度计对k时刻房间温度的测量,Kalman滤波是站在温度计角度进行的,
%它不知道此刻真实状态X(k),只能利用本次测量值Z(k)和上一次估计值
% Xkf(k)来做处理,其目标是最大限度地降低测量噪声R的影响,尽可能
%地逼近X(k),这也是Kalman滤波目的所在
Z(k)=H*X(k)+V(k);
%第三步: Kalman滤波
%有了k时刻的观测Z(k)和k-1时刻的状态,那么就可以进行滤波了,
%读者可以对照式(3.36) 到式(3.40) ,理解滤波过程
X_pre=F*Xkf(k-1);%状态预测
P_pre=F*P(k-1)*F'+Q;%协方差预测
Kg=P_pre*inv(H*P_pre*H'+R); %计算Kalman增益
e=Z(k)-H*X_pre;%新息
Xkf(k)=X_pre+Kg*e; %状态更新
P(k)=(I-Kg*H)*P_pre;
%协方差更新
end
%%%%%%%% %%%%%%%%%%%%%%%%% %%%%%%%%%%%
%%%%%%%%%
%计算误差
Err_Messure=zeros(1,N);% 测量值与真实值之间的偏差
Err_Kalman=zeros(1,N);% Kalman估计与真实值之间的偏差
for k=1:N
Err_Messure(k)=abs(Z(k)-X(k));
Err_Kalman(k)=abs(Xkf(k)-X(k));
end
t= 1:N;
% figure("Name','Kalman Filter Simulation';'Number Title','off);
figure(1) %画图显示
%依次输出理论值,叠加过程噪声(受波动影响)的真实值,
%温度计测量值,kalman估计值
plot(t,Xexpect,'-b',t,X,'-.r',t,Z,'-ko',t,Xkf,'-g*');
legend('期望值','真实值','观测值','Kalman滤波值');
title('KalmanFilter预测图')
xlabel('采样时间/s');
ylabel('温度值/℃');
figure(2) %误差分图
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k*');
title('KalmanFilter误差图')
xlabel('采样时间/s');
ylabel('温度值/℃');
注:欢迎点赞收藏和评论。