卡尔曼滤波模型及Matlab模型建立

这篇具有很好参考价值的文章主要介绍了卡尔曼滤波模型及Matlab模型建立。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

目录

一、卡尔曼滤波

1.概念解析:

2.卡尔曼滤波的最优估计模型

3.实例——小车匀加速直线运动

4.Matlab建模

 二、扩展卡尔曼滤波(EKF:Extended KAlman Filter)

1.非线性系统的局部线性化

2.扩展卡尔曼滤波模型


一、卡尔曼滤波

        卡尔曼滤波的目的:由于人的主观认识(数学模型的建立而产生的理论状态值)和测量(传感器等测量值)都不准确,引入卡尔曼滤波,综合两者的误差,得到最优的对于真实值的预测。

1.概念解析:

        对于已知状态空间表达卡尔曼滤波模型及Matlab模型建立线性系统,通过递归算法,基于对下一时刻状态估计误差与观测器的测量误差数据融合得到下一时刻状态的最优预测值

        其中,引入估计误差的协方差矩阵P表征状态变量之间的相关性;引入系统误差w和观测误差v表征在预测及测量过程中会存在偏差的实际情况。由于任何外部状态通常呈现正态分布,假设,(即w服从以0为期望,Q为协方差矩阵的正态分布;v服从以0为期望,R为协方差矩阵的正态分布)。

        状态方程中,表示第k时刻的状态量,表示第k时刻的输入量,表示k时刻的观测值,矩阵H为观测矩阵。

概念框图:卡尔曼滤波模型及Matlab模型建立

2.卡尔曼滤波的最优估计模型

        1)预测:①先验卡尔曼滤波模型及Matlab模型建立

                        ②先验误差协方差卡尔曼滤波模型及Matlab模型建立

        2)  校正:①卡尔曼增益卡尔曼滤波模型及Matlab模型建立

                        ②后验估计卡尔曼滤波模型及Matlab模型建立

                        ③更新误差协方差

        公式推导详见:(DR_CAN老师讲解的所有系列都超级推荐!!!)【卡尔曼滤波器】3_卡尔曼增益超详细数学推导 ~全网最完整_哔哩哔哩_bilibili

        简单解释:①k时刻的先验估计为上一时刻的最优预测值;

                        ②误差协方差矩阵P用来描述该时刻状态变量间的联系;Q为噪音矩阵,意思是变量间关系也受外界影响;

卡尔曼滤波模型及Matlab模型建立

                         ③R为观测矩阵,卡尔曼增益的大小取决于系统变量间的关系(协方差矩阵P)及观测器的性能(H、R)

                        ④后验估计值对观测结果的依赖程度体现在,目的是让最优估计值的方差更小

                        ⑤每一帧状态变量改变后,变量间的关系随之改变,故要时刻更新。

3.实例——小车匀加速直线运动

卡尔曼滤波模型及Matlab模型建立

 卡尔曼滤波模型及Matlab模型建立

4.Matlab建模

        假设模型参数:过程预测噪声的协方差矩阵,

观测噪声的协方差矩阵,状态方程中,,观测矩阵

        初始状态:,,,则代码如下:

%%先对不同变量进行定义
    %Q为过程预测噪声的协方差矩阵
    %R为观测噪声协方差矩阵
    %状态变量X=[p v],p为小车的位置,v为速度;输入u为加速度(=外力/质量)
    %X为实际状态
    %X_bar 为先验估计
    %Xbar为后验估计,最优估计值
    %P_为先验估计误差的协方差矩阵
    %P为后验估计误差的协方差矩阵
    %Z为观测量
    %K为卡尔曼增益
    
%%定义超参数
over = 50;%指定循环的遍数,即预测50s内的运动
Q = [0.1 0;0 0.1];%假设的过程预测噪声的协方差矩阵(实际上是一个变量,很难得到)
R = [1 0;0 1];%假设的观测噪声协方差矩阵(实际上也是变量,但是可以得到)

%定义尺寸参数
T = [over,2];%定义over行2列(2维状态)的矩阵,用于记录所有时刻的预测状态
delta_t = 1;%时间间隔

%假设小车为匀加速运动,加速度为1
u = 1;

%建立预测模型
A = [1 delta_t;0 1];
B = [(delta_t^2)/2;delta_t];

H =[1 0;0 1];%假设观测值的观测矩阵

I = [1 0;0 1];%定义单位矩阵

%初始化参数,由于Kk、Pk为二维矩阵,故包含所有迭代数据的K、P均为2行T列矩阵
X = zeros(T);
X_bar = zeros(T);
Xbar = zeros(T);
Z = zeros(T);
K = zeros([2*over,2]);
P = zeros([2*over,2]);
P_ = zeros([2*over,2]);

%在t=0时刻的初始值
P(1:2,:)=[1 0; 0 1];%初始前两行协方差矩阵为[1 0;0 1],表示状态变量间相互独立
X(1,:) = [0 1];%实际状态初始值,即假设初始位置为0,初始速度为1
Xbar(1,:) = [0 1];%状态预测量的初始值;my error:①Xbar(1)表示矩阵的第一个元素②Xbar(1,:)可以表示第一行,注意是冒号:

%%卡尔曼滤波的核心算法
for n = 2:over
    %计算X的实际值:X(k)=AX(k-1)+W
    w1 = normrnd(0,sqrt(Q(1)),[1,1]);%w1是以0为均值,Q矩阵第一个元素的开方为标准差的服从正态分布的随机数
    w2 = normrnd(0,sqrt(Q(4)),[1,1]);%w2是以0为均值,Q矩阵右下方元素的开方为标准差的服从正态分布的随机数
    W = [w1 w2];%过程估计误差矩阵
    Tmp1 = A*X(n-1,:)'+B*u+W';
    X(n,:) = Tmp1';%在t=n时刻的状态实际值X(n)
    
    %计算状态的观测值:Z(k)=HX(k)+V
    v1 = normrnd(0,sqrt(R(1)),[1,1]);%v1是以0为均值,R矩阵左上角元素的开方为标准差的服从正态分布的随机数
    v2 = normrnd(0,sqrt(R(4)),[1,1]);%v2是以0为均值,R矩阵右下方元素的开方为标准差的服从正态分布的随机数
    V = [v1 v2];%观测误差矩阵
    Tmp2 = H*X(n,:)'+V';
    Z(n,:) = Tmp2';%在t=n时刻的状态观测值Z(n)
    
    %先验预测:X_bar(k)=AXbar(k-1)+Bu(k-1)
    Tmp3 = A*Xbar(n-1,:)'+B*u;%更新先验估计,k时刻的先验模型为上一时刻(k-1)的状态最优模型;注意计算时用X与u的转置,因为状态方程中状态x与输入u是列向量
    X_bar(n,:) = Tmp3';
    
    P_(((2*n-1):(2*n)),:) = A*P(((2*n-3):(2*n-2)),:) *A'+Q;%更新先验估计误差协方差:P_bar(k)=A*Pbar(k)*A'+Q
    
    %后验校正
    %卡尔曼增益
    K(((2*n-1):(2*n)),:) = P_(((2*n-1):(2*n)),:)*H'/(H*(P_(((2*n-1):(2*n)),:)*H' + R));
    %后验估计预测值:Xbar(k)=X_bar(k)+K(Z(k)-H*X_bar(k))
    Tmp4 = X_bar(n,:)' + K(((2*n-1):(2*n)),:)*(Z(n,:)'-H*X_bar(n,:)');
    Xbar(n,:) = Tmp4';
    
    P(((2*n-1):(2*n)),:) = (I-K(((2*n-1):(2*n)),:)*H)*P_(((2*n-1):(2*n)),:);%协方差矩阵P(k)更新:P(k)=(I-KH)*P_bar(k)
end

%%绘图
figure
LineWidth = 2;
%分裂窗口为2*1个子窗口
%位置曲线图
subplot(2,1,1)
plot(Z(:,1),'k-')%画出测量值
hold on;

plot(Xbar(:,1),'b-')%画出最优估计值
hold on;

plot(X(:,1),'y-')%画出实际状态值
hold on;
title('位置状态曲线')

legend('位置测量值','位置最优估计值','实际位置')%同一图像中包含多条曲线时,依次加标注
%速度曲线图
subplot(2,1,2)
plot(Z(:,2),'k-')%画出测量值
hold on;

plot(Xbar(:,2),'b-')%画出最优估计值
hold on;

plot(X(:,2),'y-')%画出实际状态值
hold on;
title('速度状态曲线')

legend('速度测量值','速度最优估计值','实际速度')

运行的状态曲线为:

卡尔曼滤波模型及Matlab模型建立 

 

注:Q,R的数值不断调整,得到的估计效果不同

        参数寻优:使最优估计值更好地拟合实际值。

 二、扩展卡尔曼滤波(EKF:Extended KAlman Filter)

        对于非线性系统,其状态空间不能表示为卡尔曼滤波模型及Matlab模型建立的形式,则上述卡尔曼滤波模型不成立,并且正态分布的随机变量经过非线性系统的处理后,不再满足正态分布(Q,R也应作相应改变)。

1.非线性系统的局部线性化

        在操作点附近进行局部线性化(泰勒展开),得到非线性系统的最优预测模型——扩展卡尔曼滤波模型:

        对于系统,f、h均为非线性函数

                由于系统存在估计和观测等误差,无法在真实点进行线性化,模型中选取f在(k-1)时刻的后验估计点处进行线性化:

卡尔曼滤波模型及Matlab模型建立

卡尔曼滤波模型及Matlab模型建立

                其中,为估计误差为0时的预测点,即;矩阵均为雅克比矩阵做系数矩阵,且随k变化而时刻更新

                ,,,

        综上,得到的线性系统为:

卡尔曼滤波模型及Matlab模型建立

        其中,,.

        对做线性变换的仍满足正态分布,即,。

2.扩展卡尔曼滤波模型

        将线性化后的模型代入卡尔曼滤波模型,有:

卡尔曼滤波模型及Matlab模型建立

(推荐大家观看b站up主:DR_CAN  的视频,并和我一起交流学习经验,欢迎批评指正!!)文章来源地址https://www.toymoban.com/news/detail-409461.html

到了这里,关于卡尔曼滤波模型及Matlab模型建立的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 线性卡尔曼跟踪融合滤波算法(Matlab仿真)

            卡尔曼滤波的原理和理论在CSDN已有很多文章,这里不再赘述,仅分享个人的理解和Matlab仿真代码。         假设目标的状态为X =  [x, y, vx, vy],符合匀速直线运动目标,也即                  其中F为状态转移矩阵,                  在匀速直线(const velo

    2024年01月17日
    浏览(43)
  • 状态空间模型与卡尔曼滤波

    1)说起卡尔曼滤波,必有状态空间模型,两个离不开。 2)从卡尔曼滤波名字就可以看出来,其更倾向于滤波。即对系统噪声和测量噪声进行过滤优化,得出更优的结果。如果系统噪声比较强,那么最终结果就会倾向于测量结果,而当测量噪声强时,最终结果就倾向于系统状

    2024年02月03日
    浏览(40)
  • 滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现

    本文接着分享位姿跟踪和滤波算法中用到的一些常用程序,希望为后来者减少一些基础性内容的工作时间。以往分享总结见文章:位姿跟踪 | 相关内容目录和链接总结(不断更新中~~~) 本文分享无迹卡尔曼滤波(UKF)算法的一些基本公式和MATLAB程序。 首先简单介绍一下UKF滤

    2023年04月15日
    浏览(49)
  • 40行MATLAB代码实现卡尔曼滤波-简单易懂

    最近学习了卡尔曼滤波,体会到了数据融合下进行最优估计的思想。如果你也是小白,可以通过这个例子自己动手感受数据融合。 学习资料参考B站大神DR_CAN博士,连接: 【卡尔曼滤波器】直观理解与二维实例 基于上述视频中Excel的例子,使用MATLAB编写了一个简单的卡尔曼滤

    2024年02月04日
    浏览(50)
  • 卡尔曼滤波器原理讲解及其matlab实现

    目录 一:卡尔曼滤波器的信号模型[1-2] 二:其他方程及变量介绍 三:卡尔曼滤波器递推公式 四:matlab仿真[3] 参考文献: 引言:在进行一些信号处理的过程中,我们通常会采集到一些数据,但是实际测量到的数据是受到噪声干扰了之后的,故与真实的数据有一些偏差。因此

    2023年04月08日
    浏览(45)
  • 【状态估计】基于卡尔曼滤波器和扩展卡尔曼滤波器用于 INS/GNSS 导航、目标跟踪和地形参考导航研究(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果  2.1 算例1 ​ 2.2 算例2  2.3 算例3 🎉3 参

    2024年02月11日
    浏览(52)
  • 基于变分贝叶斯的自适应卡尔曼滤波(matlab)

        参考文献《Recursive Noise Adaptive Kalman Filtering by Variational Bayesian Approximations》中的算法,用matlab编写了一个小程序测试了一下(demo版本)。程序是按照文章中的内容实现的。     下面是主程序:     下面是调用到的几个函数:     函数一(cholesky分解,

    2024年02月11日
    浏览(45)
  • 无迹卡尔曼滤波估计SOC(附MATLAB程序详解)

    设置电流采样周期为1s   导入电流数据并求电流数据的长度。  设置参数Q为单位矩阵乘以1e-4,设置参数R=1e-5  设置协方差矩阵P0=1e-4乘以单位矩阵;初始化二阶RC模型的参数R0、Rs、Rp、Cs、Cp为一行N列的零矩阵  求解模型参数,通过辨识电池模型参数得到的各个参数与SOC之间

    2024年02月04日
    浏览(43)
  • 卡尔曼滤波 - 状态空间模型中的状态方程

    卡尔曼滤波 - 状态空间模型中的状态方程 flyfish 状态方程和观测方程统称为状态空间模型  位移 = Δ x = x f − x 0 text { 位移}=Delta x=x_f-x_0   位移 = Δ x = x f ​ − x 0 ​ x 0 x_0 x 0 ​ 是起始位置 x f x_f x f ​ 是终止位置 在坐标轴里,右边是正,左边是负 绿色矩形的高度为 v 0 v

    2024年02月02日
    浏览(41)
  • 基于卡尔曼滤波进行四旋翼动力学建模(Simulink&Matlab)

      💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码实现 文献来

    2024年02月11日
    浏览(47)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包