文章目录

前言
一、基础知识
- 1、思维导图
- 2、控制基础知识
- - ①线性系统
  - ②状态空间表达式
- 3、数学基础知识
- - ①高斯分布(正态分布)
  - - 一维
    - 二维
  - ②一维协方差性质
二、卡尔曼公式推导
- 1、思维导图
- 2、实际公式
- - 预测(据上一次结果来推)
  - 更新(根据观测修正预测值)
- 3、卡尔曼使用步骤
三、举例(结合实际)
四、卡尔曼程序
- .c部分程序
- .h部分程序

前言

本文是根据b站视频BV1Rh41117MT做的学习笔记。

一、基础知识

1、思维导图

2、控制基础知识

①线性系统

所谓线性系统就是满足叠加原理的系统就是线性系统。

<<<注释：这里引用一下卢老师课上将的基础知识(见b站P5的7：54)

②状态空间表达式

描述系统输入、输出和状态变量之间关系的方程组称为系统的状态空间表达式（动态方程或运动方程）

方框图表示：

注意：
1、这里图片引用该处：https://blog.csdn.net/qq_42680785/article/details/106378414
想要更加深入了解该方面的知识可以见该篇文章。
2、线性状态空间表达式在这有些区别：加入了实际的噪声（wkw_{k}wk和vkv_{k}vk ）且服从高斯分布。
3、过程噪声wkw_{k}wk产生方差QkQ_{k}Qk(具体关系见公式推导部分)
4、测量噪声vkv_{k}vk产生观测方差RkR_{k}Rk(主要由传感器决定)(用测量方程来描述关系)

3、数学基础知识

①高斯分布(正态分布)

一维

其中X服从高斯分布X∼N(μ,σ2)X\sim N\left ( \mu ,\sigma ^{2} \right )X∼N(μ,σ2)
f(x)=12πσe−(x−μ)22σ2f\left ( x \right ) =\frac{1}{\sqrt{2\pi}\sigma } e^{-\frac{{\left ( x-\mu \right ) ^{2}}}{2\sigma ^{2} } }f(x)=2πσ1e−2σ2(x−μ)2
图像：

二维

注：详细可见百度百科

②一维协方差性质

注：
1、详细可见 2023年李林概率论辅导讲义
2、二维协方差公式可自行百度

二、卡尔曼公式推导

1、思维导图

2、实际公式

预测(据上一次结果来推)

1、先验估计x^t−=Fx^t—1+But—1\hat{x} _{t}^{-} =F\hat{x} _{t—1}+Bu_{t—1}x^t−=Fx^t—1+But—1
2、先验估计协方差Pt−=FPt—1FT+Q{P} _{t}^{-} =F{P} _{t—1}F^{T} +QPt−=FPt—1FT+Q
3、测量方程(ZtZ_{t}Zt与vtv_{t}vt的关系)Zt=Hxt+vtZ_{t} =Hx_{t} +v_{t} Zt=Hxt+vt

注：Pt−{P} _{t}^{-}Pt−为协方差矩阵Pt−=cov(x^t−,x^t−){P} _{t}^{-} =cov\left ( \hat{x} _{t}^{-},\hat{x} _{t}^{-} \right )Pt−=cov(x^t−,x^t−)=cov(Fx^t−1+But−1+wt,Fx^t−1+But−1+wt)=cov\left ( F\hat{x} _{t-1}+Bu_{t-1}+w_{t} ,F\hat{x} _{t-1}+Bu_{t-1}+w_{t} \right )=cov(Fx^t−1+But−1+wt,Fx^t−1+But−1+wt)=Fcov(x^t−1,x^t−1)FT+cov(wt,wt)=Fcov\left ( \hat{x} _{t-1},\hat{x} _{t-1}\right )F^{T} +cov\left ( w_{t} ,w_{t} \right )=Fcov(x^t−1,x^t−1)FT+cov(wt,wt)=FPt—1FT+Q=F{P} _{t—1}F^{T} +Q=FPt—1FT+Q
从而可以得到QkQ_{k}Qk与wtw_{t}wt的关系
其中ut−1u_{t-1}ut−1为常数，噪声为变量，故可用性质推的上面结果。

更新(根据观测修正预测值)

1、卡尔曼增益
Kt=Pt−HT(HPt−HT+R)−1K_{t} =P_{t} ^{-} H^{T} \left ( HP_{t} ^{-}H^{T}+R \right ) ^{-1}Kt=Pt−HT(HPt−HT+R)−1
2、修正估计(最优)
x^t=x^t−+Kt(zt−Hx^t)\hat{x} _{t} =\hat{x} _{t}^{-} +K_{t} \left (z_{t} -H\hat{x} _{t}\right )x^t=x^t−+Kt(zt−Hx^t)
3、协方差更新
Pt=(I−KtH)Pt−P_{t}=\left ( I- K_{t}H\right ) P_{t}^{-}Pt=(I−KtH)Pt−

注：其中卡尔曼增益公式可化简为(H为1,F为1)：
Kt=Pt−HT(HPt−HT+R)K_{t}=\frac{P_{t} ^{-} H^{T} }{\left ( HP_{t} ^{-}H^{T}+R \right )} Kt=(HPt−HT+R)Pt−HT
=Pt−(Pt−+R)=\frac{P_{t} ^{-}}{\left ( P_{t} ^{-}+R \right )} =(Pt−+R)Pt−
=FPt—1FT+Q(FPt—1FT+Q+R)=\frac{F{P} _{t—1}F^{T} +Q}{\left (F{P} _{t—1}F^{T} +Q+R \right )} =(FPt—1FT+Q+R)FPt—1FT+Q
=Pt—1+Q(Pt—1+Q+R)=\frac{{P} _{t—1} +Q}{\left ({P} _{t—1}+Q+R \right )} =(Pt—1+Q+R)Pt—1+Q

注：
1、由上述推导过程可以看出来Q和R可以影响K值，从而影响观测值在最终滤波结果中的比重。比如，我用更好的传感器，这时候我更信任观测值。R变小→K变大→增加比重。

3、卡尔曼使用步骤

1、选择状态量、观测量
2、构建方程
3、初始化参数
4、带入公式迭代
5、调节超参数(QkQ_{k}Qk、ZtZ_{t}Zt)

三、举例(结合实际)

注：这里的P[0][0]是指对应矩阵的第0行，第0列。

四、卡尔曼程序

.c部分程序

float kalman(float Gyro_speed,float Mma_speed)
{Zinc_f = Zinc_b + Ts*Gyro_speed; P_f[0][0] = P_b[0][0] - Ts*(P_b[0][1] + P_b[1][0]) + Ts*Ts*P_b[1][1] + Qinc;P_f[1][0] = P_f[0][1] = P_b[0][1] - Ts*P_b[1][1];P_f[1][1] = P_b[1][1] + Qgyro;y_k = Mma_speed - Zinc_f;k_1    = P_f[0][0] / (P_f[0][0] + Rmeans);k_2    = P_f[1][0] / (P_f[0][0] + Rmeans);Zinc_b = Zinc_f + k_1*y_k;U_gyro = U_gyro + k_2*y_k;P_b[0][0] =   (1- k_1)*P_f[0][0];P_b[0][1] = (1- k_1)*P_f[0][1];P_b[1][0] = -1*k_2*P_f[0][0] + P_f[0][1];P_b[1][1] =  -1*k_2*P_f[0][1] + P_f[1][1];return Zinc_b;
}

.h部分程序

#ifndef _kem_H
#define _kem_H
#include "stm32f10x.h"
float kalman(float Gyro_speed,float Mma_speed);
#endif

卡尔曼滤波原理学习笔记相关推荐

MOOC人工智能原理学习笔记1
人工智能原理学习笔记1 The Foundations of AI: Philosophy Mathematics Economics Neuroscience Psychology Computer ...
自控原理学习笔记-反馈控制系统的动态模型（4）-频率特性函数Nyquist图及Bode图
自控原理学习笔记自控原理学习笔记专栏文章目录 1.频率特性函数 1.1 图形表示方法: 1.2 零极点位置和暂态增益图 1.2.1 复轨迹曲线 1.2.3 例子 1.3 计算系统响应 2.开环频率 ...
自控原理学习笔记-系统稳定性分析（2）-环路分析及Nyquist-Bode判据
自控原理学习笔记自控原理学习笔记专栏文章目录 3. 环路分析 3.1环路分析基本思想: 3.2 稳定程度的性能指标(相对稳定) 3.3 环路整形 4.Nyquist判据 4.1 与幅角原理关系 4 ...
Golang底层原理学习笔记（一）
LCY~~Golang底层原理学习笔记 1 源码调试 go源代码地址:GitHub - golang/go: The Go programming language 1.1 源码编译现在的go语言大 ...
[编译原理学习笔记2-2] 程序语言的语法描述
[编译原理学习笔记2-2] 程序语言的语法描述文章目录 [编译原理学习笔记2-2] 程序语言的语法描述 [2.3.1] 上下文无关文法 [2.3.2] 语法分析树与二义性 [2.3.3] 形式语言鸟 ...
自控原理学习笔记-反馈控制系统的动态模型（1）
自控原理学习笔记 1.导论 2.反馈控制系统的动态模型(1) 3.反馈控制系统的动态模型(2) 3.反馈控制系统的动态模型(3) 4.反馈控制系统的动态模型(4) 5.反馈控制系统的动态模型(5) 文 ...
Spring5底层原理学习笔记（二）AOP篇
文章目录 AOP实现之ajc编译器 AOP实现之agent类加载 AOP实现之动态代理 jdk动态代理演示模拟实现动态代理动态生成代理类需要使用到asm的api,这里就不展开了 Jdk对于反射调 ...
编译原理学习笔记20——符号表
编译原理学习笔记20--符号表 20.1 符号表的组织与操作 20.2 符号表的内容 20.3 利用符号表分析名字的作用域 20.1 符号表的组织与操作符号表符号表的作用与组织符号表的整理和查找 ...
操作系统原理学习笔记（二十一）-对换
文章首发及后续更新:https://mwhls.top/1350.html 新的更新内容请到mwhls.top查看. 无图/无目录/格式错误/更多相关请到上方的文章首发页面查看. 操作系统原理学习笔记 ...
编译原理学习笔记2——高级程序设计语言概述
编译原理学习笔记2--高级程序设计语言概述 2.1常用的高级程序设计语言 2.2程序设计语言的定义 2.2.1语法 2.2.1语法 2.2.3程序语言的基本功能和层次机构 2.2.4程序语言成分的逻辑 ...

卡尔曼滤波原理学习笔记

文章目录

前言

一、基础知识

1、思维导图

2、控制基础知识

①线性系统

②状态空间表达式

3、数学基础知识

①高斯分布(正态分布)

一维

二维

②一维协方差性质

二、卡尔曼公式推导

1、思维导图

2、实际公式

预测(据上一次结果来推)

更新(根据观测修正预测值)

3、卡尔曼使用步骤

三、举例(结合实际)

四、卡尔曼程序

.c部分程序

.h部分程序

卡尔曼滤波原理学习笔记相关推荐

最新文章

热门文章