三个实例迅速掌握经典卡尔曼滤波用法

Kalman滤波通俗理解+实际应用 一、Kalman用于解决什么的问题? 卡尔曼滤波是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。 人话: 线性数学模型预测值+传感测量值=更准确的测量值。 二、先来看一下姿态估计问题 ... 阅读详情
AI摘要

本文通过三个典型实例——室内温度测量、自由落体运动跟踪和弹簧-阻尼系统状态估计,详细演示了经典卡尔曼滤波器的应用方法。涵盖状态预测、更新及噪声处理等核心步骤,结合MATLAB/Simulink实现,帮助快速掌握其在动态系统状态估计中的实际用法。

一、经典卡尔曼滤波器

假设我们系统(线性的)的离散状态空间方程如下,其中,ABCD是状态矩阵,w(k)和v(k)分别代表过程噪声和系统噪声,假设过程噪声和测量噪声的房差分别为Q和R。

在这个状态空间方程基础上,我们可以使用经典卡尔曼滤波器进行状态观测。

1、状态预测

带帽子的x表示估计的意思,右上角再加-表示先验估计状态(纯计算)。直观上理解,就是拿上一刻的滤波结果hat_x(k-1)和输入u(k-1),直接套公式,算出一个理论的估算结果。

2、误差协方差预测

引入的过程噪声方差Q,直观上可以理解为,对状态预测结果精度的评价。

3、卡尔曼增益计算

卡尔曼滤波的关键所在,计算权重系统,决定观测值对最终结果的影响程度,Kk的值在0到1之间。

4、状态更新

卡尔曼滤波灵魂所在,直观上理解,如果Kk的值为0,则,表示完全相信套公式计算的结果;如果Kk的值为1,且正常情况下,C元素是1,则,完全相信测量的结果;当Kk在0~1之间时,则是测量结果与计算结果的融合。

5、误差协方差更新

更新误差协方差矩阵,进入下一轮循环。

至于这五个公式怎么来的,学有余力的同学可以b站学习下总线博士的视频,至于博主嘛,先用了再说,推导是科学家们的事。

DR_CAN的个人空间-DR_CAN个人主页-哔哩哔哩视频 (bilibili.com)

 二、实例一:测量室内温度

假设室内温度初始温度是25度。但是由于透风、或者晒太阳,温度会有波动,这个波动叫过程噪声,我们假设过程噪声方差Q=0.01;我们拿温度计测量室内温度,温度计也有精度误差,再加上我们眼花可能也会看错,这个误差我们假设在±0.5℃上波动,测量噪声方差R=0.25;这是一个一维系统,状态空间方程如下:

即:A=1,B=0,C=1,D=0,Q=0.01,R=0.25; 下面开始套参环节:

clc;clear;close all;
% 系统参数
Temp=25;          %房间初始温度
dt=0.001;

% 状态空间模型
A=1;
B=0;
C=1;
D=0;

%卡尔曼滤波器初始化
P = 1;             % 估计协方差矩阵初始值
Q = 0.01;          % 过程噪声协方差矩阵
R = 0.25;          % 测量噪声协方差
x_est(1) = 25;    % 状态估计初始值
I=eye(length(x_est));

% 模拟数据
t = 0:dt:10;
x_true = zeros(1, length(t)); %存放真实值
z = zeros(size(t));           %存放测量值

% 初始条件
x_true(1) = Temp;
z(1) = C*x_true(:,1) + sqrt(R)*randn;

% 模拟系统和测量
for i = 2:length(t)
    % 真实系统更新,计算机模拟系统
    x_true(i) = A*x_true(i-1) + sqrt(Q)*randn;
    % 生成测量数据
    z(i) = C*x_true(i) + sqrt(R)*randn;          
    % 调用卡尔曼滤波更新
    [x_est(i), P] = kalman_filter(x_est(i-1), 0, P, z(i), A, C, Q, R,B); 
end
figure
hold on;grid on;box on;
plot(t,z); % 真实位移
plot(t,x_true); % 真实位移
plot(t,x_est);  % 滤波后的位移
xlabel('时间 s');
ylabel('位移  m');
title('卡尔曼滤波 位移x')
legend('x mea','x true','x est');

function [x_est, P] = kalman_filter(x_est,u, P, z, A, C, Q, R, B)
    % 预测
    x_pre = A*x_est + B*u;
    P_pre = A*P*A' + Q;

    % 更新
    K = P_pre*C' / (C*P_pre*C' + R);
    x_est = x_pre + K * (z - C*x_pre);
    P = (eye(length(x_est)) - K*C) * P_pre;
end

运行结果如下:

 估算的结果与真实结果非常接近。相比测量结果,少了很多噪声。至于温度为什么上升了,跟matlab生成随机数randn函数有关系。

显然,这是在MATLAB脚本里仿真,总感觉跟真实系统相比差点意思?我们把卡尔曼滤波这五个公式搬进simulink模型,并将房间温度模拟当做一个实体plant,如下图所示(也可以用simulink自带的kf模块)。

 运行结果如下:

 三、实例二:自由落体物体实例

假设一个小球从1000m的高空自由落体,忽略空气阻力,只受重力影响。地面有个雷达站,实时观测小球的位置,下面我们建立状态空间方程

 设物体的位移s,物体的速度为v

是假设Q1=Q2=0.01;R=1; 接下来跟上一个实例类似,进入套参数环节。

clc;clear;close all; 
% 系统参数
dt=0.001;
InitialHigh=1000;
u=-10;
% 状态空间模型
A=[1 dt;0 1];
B=[0.5*dt*dt;dt];
C=[1 0];
D=0;

%卡尔曼滤波器初始化
P = [1 0;0 1];             % 估计协方差矩阵初始值
Q = [0.01 0;0 0.01];       % 过程噪声协方差矩阵
R = 1;                     % 测量噪声协方差
x_est(:,1) = [InitialHigh;0];% 状态估计初始值
I=[1 0;0 1];

% 模拟数据
t = 0:dt:10;
x_true = zeros(2, length(t)); %存放真实值
z = zeros(size(t));           %存放测量值

% 初始条件
x_true(:,1) = [InitialHigh;0];
z(1) = C*x_true(:,1) + sqrt(R)*randn;

% 模拟系统和测量
for i = 2:length(t)
    % 真实系统更新,计算机模拟系统
    x_true(:,i) = A*x_true(:,i-1) + B*u+sqrt(Q)*randn(2,1);
    % 生成测量数据
    z(i) = C*x_true(:,i) + sqrt(R)*randn;          
    % 调用卡尔曼滤波更新
    [x_est(:,i), P] = kalman_filter(x_est(:,i-1), u, P, z(i), A, C, Q, R,B); 
end
figure
hold on;grid on;box on;
plot(t,z); % 真实位移
plot(t,x_true(1,:)); % 真实位移
plot(t,x_est(1,:));  % 滤波后的位移
xlabel('时间 s');
ylabel('高度  m');
title('卡尔曼滤波 高度h')
legend('h mea','h true','h est');

figure
hold on;grid on;box on;
plot(t,x_true(2,:)); % 真实速度
plot(t,x_est(2,:));  % 滤波后的位移
xlabel('时间 s');
ylabel('速度  m/s');
title('卡尔曼滤波 速度v');
legend('v true','v est');

sim KalModel2.slx;

function [x_est, P] = kalman_filter(x_est,u, P, z, A, C, Q, R, B)
    % 预测
    x_pre = A*x_est + B*u;
    P_pre = A*P*A' + Q;

    % 更新
    K = P_pre*C' / (C*P_pre*C' + R);
    x_est = x_pre + K * (z - C*x_pre);
    P = (eye(length(x_est)) - K*C) * P_pre;
end

在simulink模型中

从这个实例中,可以体会到前面文章提到的观测器味道了。测量一个状态量,通过这个状态量和输入,估算另一个状态量。

四、实例三:弹簧-阻尼系统

如下图所示的弹簧阻尼系统,通过牛顿第二定律,建立微分方程,得到连续状态空间方程。

 在这个系统中,测量位移x,估计速度v;假设过程噪声Q = 0.01 * eye(2),测量噪声R=1;假设质量M=10; k=150;c=10。与前两个例子不同,在套用卡尔曼滤波器之前,需要将连续系统先离散化。输入力F可以设定输入正弦信号。

 在simulink中,同样套用前两个例子的模型,运行得到结果。

五、关于脚本和模型

关键步骤脚本和模型框架已经贴出,如果需要源模型和代码,还是在某宝店<极简车辆控制>中。

 

 

定位算法——Kalman滤波简要入门(理论+实例+matlab实现) 卡尔曼滤波是基于概率推理的方法,它通过融合系统的预测模型和测量数据来估计系统的状态,尤其适用于带有噪声的动态系统。在每个时间步骤中,卡尔曼滤波主要包含预测步骤和更新步骤 阅读详情

相关推荐

组合导航(七):卡尔曼滤波

1.Kalman滤波 1.Kalman滤波 惯性传感器在初始化以及算法的误差影响惯导系统的精度。低成本MEMS传感器由于严重的随机误差,INS输出可能迅速漂移因此,因此低精度的IMU基本上不能作为导航的独立传感器进行应用。 传感器的主要误差是加速度计偏差和陀螺漂移。为了提高惯导系统的精度,必须采用k个规则时间间隔来估计惯性传感器的随机误差,作为补偿的基础。如水平通道上的速度和姿态角的例子中所述。...

weixin_43793960的博客 1万+

卡尔曼滤波的使用举例

卡尔曼滤波(Kalman filtering)一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。 顺带提一下协方差矩阵,下面会用到。 协方差矩阵: 以下来源:https://blog.youkuaiyun.com/su_bao/article/details/80997077 ...

VictorHan01的博客 9403

卡尔曼滤波算法以及算法拓展开发详解:原理剖析与C语言实现

本文深入剖析卡尔曼滤波算法的数学原理、推导过程及工程实现,提供完整C语言代码和参数调优指南,并详细介绍多种变种算法的实现方法。

汽车电子软件开发笔记 1290

卡尔曼滤波器设计及实例

其发明者鲁道夫.E.卡尔曼在一次访问NASA的时候,发现阿波罗计划中一个难点是轨道预测问题,因而提出了一种滤波器,可以帮助高效预测轨迹,辅助导航。即便在有噪声干扰的情况下,也可以较好的预测下一状态的情况,并找出多变量间不易察觉的相关性。因而卡尔曼滤波器可以很好适应不断变化的系统,并且内存占用量低,推理速度快,比较适合资源受限制的场景。假设有一辆小车,装有加速度传感器,可以测量小车的加速度a。其中u(k)是系统控制输入,w(k)是系统随机噪声。其中P和Q是与转移矩阵A同秩的协方差方阵,R是1X1维的常量。

1630

卡尔曼滤波参数是如何影响滤波效果的?——KF第一篇笔记

卡尔曼滤波的参数

985工科博士毕业,专攻定位、导航和滤波等算法研究。从业10年,主要使用MATLAB 8426

基于卡尔曼滤波的组合导航系统设计与实现

在现代导航系统中,组合导航技术已成为实现高精度定位的关键技术。它综合多种导航传感器的数据,如全球定位系统(GPS)和惯性导航系统(INS),以提供连续且可靠的导航信息。组合导航系统通过各种算法将不同类型的传感器数据融合,以弥补单一传感器的局限性,提高整个导航系统的准确性和鲁棒性。组合导航技术的核心优势在于它能够克服单一传感器的缺点。例如,GPS信号在城市峡谷或室内环境中可能会受到干扰,而INS则可以提供连续的导航信息,但它在没有外部校正的情况下会逐渐积累误差。

weixin_34471637的博客 1278

卡尔曼滤波详解:一维卡尔曼滤波实例解析(五个公式以及各个参数的意义)

一、引言 本文以rssi(接收信号强度)滤波为背景,结合卡尔曼的五个公式,设计 rssi 一维卡尔曼滤波器,用MATLAB语言实现一维卡尔曼滤波器,并附上代码和滤波结果图; 本文工分为以下几个部分: 1、引言 2、模型的系统方程和状态方程 3、卡尔曼滤波过程及五个基本公式 4、公式中每个参数详细注释 5、结合rssi滤波实例设计滤波器 6、MATLAB实现滤波器 二、模型的...

u012912039的专栏 3万+

kalman滤波超详细讲解加小例子

kalman滤波超详细讲解加小例子,非常适合初学者和研究人员

【状态估计】卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、库图尔卡尔曼滤波、M-估计、鲁棒立方卡尔曼滤波器实现无人机位置跟踪、迎角和俯仰角跟踪, 方向角度跟踪等研究附Matlab代码

无人机(Unmanned Aerial Vehicle, UAV)技术在近些年来取得了显著发展,其应用领域日益广泛,涵盖了物流运输、环境监测、农业植保、灾害救援等多个方面。实现无人机安全、稳定、高效的自主飞行,离不开精确可靠的状态估计。状态估计的目标是根据传感器量测数据,实时准确地估计无人机的位置、速度、姿态等状态信息,为后续的控制和决策提供基础。然而,实际飞行环境中,无人机传感器易受噪声干扰,量测模型存在非线性,以及可能出现的异常值等因素,对状态估计的精度和鲁棒性提出了严峻挑战。

m0_60703264的博客 825

人工智能训练师技能等级认定四级技能考核试卷-样题1

本文为人工智能训练师四级技能考核试卷,包含六个实操考核项目,总分100分,考试时间150分钟。试卷涵盖数据处理、模型训练准备及系统维护等核心技能:1)垃圾分类数据集规范与处理(30分),要求评估数据格式并编写Python处理代码;2)学生成绩数据集分析(20分),涉及数据预处理、可视化及相关性分析;3)数据标注审核(10分),重点审核标注准确性;4)智能数据服务系统维护与优化(40分),包括安全帽数据集的项目创建、数据处理及平台协作。考核强调Python编程、数据处理平台操作及AI工作流程的实践能力。

weixin_42221934的博客 442

AI Agent实战:WorkBuddy安装部署、任务执行与模型接入全教程

AI Agent是当前自动化办公与智能工作流的核心技术,它区别于传统问答式AI,能够理解目标、拆解任务并调用工具交付完整成果。其底层原理基于任务规划、工具调用与结果验证的循环机制,让机器从“提供建议”进化为“完成任务”。在工程实践中,合理配置模型服务(如接入DeepSeek)、设计标准化Skill(技能包)与自定义指令,可以显著提升Agent在报告生成、数据分析、竞品调研等场景中的稳定性与效率。WorkBuddy作为典型的任务式AI工作台,为运营、产品、开发等多角色提供了统一入口,覆盖安装部署、任务描述公式

weixin_26712075的博客 293

基于扩展卡尔曼滤波(EKF)目标轨迹算法仿真实例

摘要:本博文主要介绍扩展卡尔曼滤波算法在目标轨迹跟踪中的应用。首先简单介绍了EKF算法的原理,随后通过具体的雷达仿真案例加深各位伙伴对EKF滤波算法的理解。

Poulen的博客 2215

吃透 10GBASE-T1 PHY:从 PMA 原理到线束 PCB 工程落地全指南(上)

本文介绍了以太网物理层关键子层PMA(物理介质附属子层)的功能及其测试规范。PMA位于PCS与PMD之间,负责数字信号与模拟信号的转换,包括串并转换、时钟恢复等核心功能,并支持链路监测与调试。文章详细阐述了7种测试模式的定义与应用场景,涵盖发射机抖动、线性度、功率谱密度等参数的检测方法。测试模式包括特定信号序列的发送与验证,如JP03A/B、PRBS13Q等。此外还介绍了测试夹具的使用要求,为PMA的性能验证提供了完整的技术框架。

一只豌豆象的博客 158

GNSS/INS组合导航(七):卡尔曼滤波

Kalman滤波 导航系统的精度受到 惯性传感器初始化以及算法的误差影响。低成本MEMS传感器由于严重的随机误差,INS输出可能迅速漂移。因此低精度的IMU基本上不能作为导航独立传感器进行使用。 在之前的章节中曾叙述过,对于水平面的速度与姿态角而言,若没有对速度进行持续的优化以消除速度误差,在长时间运动情况下会导致Vn和Ve值产生巨大的误差。同时,姿态角的误差,也需要依靠这些速度值来调节 ——【舒勒效应】。因此,建立精确的速度分量模型是十分重要的(可消除pitch、roll角的误差)。 几类典型的测量

努力努力努力 1万+

【5】GD32H7xx CAN发送及FIFO接收

GD32H7xx CAN收发调试记录,包含源码

madao1234的博客 1818

vLLM显存逆向计算:如何得到最优gpu-memory-utilization参数

本文提出了一种**逆向工程**的思路:基于vLLM显存占用的底层机制,通过精确计算模型权重、KV缓存、激活内存等各组件的显存需求,**倒推出最优的gpu-memory-utilization参数**。这种方法让我们能够在部署前就准确预估资源需求,实现一次配置、稳定运行。本文我们将从vLLM显存计算的核心原理出发,构建完整的倒推计算框架和源码,并通过大量实验验证其准确性(会有误差,但基本可以直接使用)。

羊城迷鹿的博客 1953

SLAM(七)-卡尔曼滤波

SLAM(七)-卡尔曼滤波 卡尔曼滤波(KF,kalman filter)是一种线性滤波方法。

weixin_41245988的博客 1195

9、硬件工程师笔试面试知识器件篇——三极管(2025版本)

本文全面介绍了三极管的基础知识与应用,包括其结构、类型、工作原理、工作状态、功能、应用场景及特性参数。详细探讨了三极管在信号放大、无线通信、音频设备及医疗设备中的具体应用,并深入分析了设计放大电路时如何平衡增益与噪声、优化偏置电路以提高稳定性、使用负反馈改善性能等关键问题。此外,文章还涵盖了高频电路设计、温度补偿策略、电磁干扰防护等实用内容,为硬件工程师提供了全面的技术参考和设计指导。

XU157303764的博客 7315

单片机:实现MPU6050的卡尔曼滤波算法(完整源码)

单片机:实现MPU6050的卡尔曼滤波算法(完整源码)

欢迎来到我的博客!这里是一个专注于计算机技术的分享平台,涵盖编程开发、算法研究、系统架构、软件工程等多个领域。无论你是初学者还是资深开发者,都能在这里找到有价值的内容。 3661

【系统架构设计师-2025上半年真题】综合知识-参考答案及详解(75题完全回忆版)

【系统架构设计师-2025上半年真题】案例分析-参考答案及部分详解

数据知道的博客 1万+

华为OD机试真题:明日之星选举 (Java/Py/C/ C++/JS/GO)

华为OD机试(机考)双机位C卷 【明日之星选举】,提供Java、C++、Python、Go、JavaScript源码实现,并提供完整思路讲解,以及在线OJ模拟做题。华为OD机试 - 明日之星选举。

qq_45776114的博客 920

多目标跟踪BOT-SORT算法(卡尔曼滤波)

卡尔曼滤波(Kalman Filter)是一种递归滤波算法,常用于在噪声和不确定性中估计系统的状态。它是基于状态空间模型,利用观测数据和系统的动态模型来估计系统的状态。卡尔曼滤波通常用于导航、控制、时间序列预测等领域。

qq_44745644的博客 3329

WINCC7.3破解文件

WINCC7.3硬件够破解文件,有需要的下载!WINCC7.3硬件够破解文件,有需要的下载!

[Probabilistic robotics] Karman卡尔曼滤波

目录内容说明基础概念例子黄金称重跟踪恒速飞机跟踪加速飞机一维卡尔曼滤波卡尔曼增益不确定性总结估算建筑物高度过程噪音例子估计罐中液体的温度估计加热液体的温度多维卡尔曼滤波器概念数学状态外推方程协方差外推测量方程状态更新方程协方差更新方程卡尔曼增益 内容说明 本文内容为https://www.kalmanfilter.net/default.aspx的学习记录 感谢原作者 基础概念 精度与准度 测量...

Uni__Lee的博客 900

SAP B1中文培訓教材_SDK

公司内部培训资料,对开发工具 ,DIAPI, UIAPI,Add-ON有详细解说

Camera2Demo

在Camera2 的基础上进行封装,使得Camera2一堆初始化逻辑与Activity分离,降低耦合度,完成预览拍照并保存的操作

卡尔曼滤波2

一篇通俗的卡尔曼滤波讲义,能让你迅速了解卡尔曼滤波的本质引自http://rocliucn.blog.sohu.com/,以下为引文  在学习卡尔曼滤波器之前,首先看看为什么叫“卡尔曼”。跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是一个人的名字,而跟他们不同的是,他是个现代人!卡尔曼全名Rudolf Emil Kalman,匈牙利数学家,1930年出生于匈牙利首

xiaobao89的专栏 1330

jre9下载 jdk9

(Java SE Runtime Environment 9.0) 9.0u175 官方最新版 【x86|x64】

腿足机器人之四- 卡尔曼滤波

机器人的任务是根据传感器读取和之前的移动历史估计自己的当前位置。这里的关键是如何根据新的传感器数据更新机器人对当前位置的估计。

shichaog的专栏 1287
上一篇: 从小车倒立摆系统看系统建模控制LQR+LQE仿真
下一篇: 史上最全的四分之一、半车再到全车7自由度常规悬架建模与仿真之一
极简车辆控制
博客等级 码龄3年 1569粉丝 39原创
评论 1
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

极简车辆控制

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值