文章目录
前言
本文是根据b站视频BV1Rh41117MT做的学习笔记。
一、基础知识
1、思维导图
2、控制基础知识
①线性系统
所谓线性系统就是满足叠加原理的系统就是线性系统。
<<<注释：这里引用一下卢老师课上将的基础知识(见b站P5的7：54)
②状态空间表达式
描述系统输入、输出和状态变量之间关系的方程组称为系统的状态空间表达式（动态方程或运动方程）
方框图表示：
注意：
1、这里图片引用该处：https://blog.csdn.net/qq_42680785/article/details/106378414
想要更加深入了解该方面的知识可以见该篇文章。
2、线性状态空间表达式在这有些区别：加入了实际的噪声（ w k w_{k} wk和 v k v_{k} vk ）且服从高斯分布。
3、过程噪声 w k w_{k} wk产生方差 Q k Q_{k} Qk(具体关系见公式推导部分)
4、测量噪声 v k v_{k} vk产生观测方差 R k R_{k} Rk(主要由传感器决定)(用测量方程来描述关系)
3、数学基础知识
①高斯分布(正态分布)
一维
其中X服从高斯分布 X ∼ N ( μ , σ 2 ) X\sim N\left ( \mu ,\sigma ^{2} \right ) X∼N(μ,σ2)
f ( x ) = 1 2 π σ e − ( x − μ ) 2 2 σ 2 f\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 − = F x ^ t — 1 + B u t — 1 \hat{x} _{t}^{-} =F\hat{x} _{t—1}+Bu_{t—1} x^t−=Fx^t—1+But—1
2、先验估计协方差 P t − = F P t — 1 F T + Q {P} _{t}^{-} =F{P} _{t—1}F^{T} +Q Pt−=FPt—1FT+Q
3、测量方程( Z t Z_{t} Zt与 v t v_{t} vt的关系) Z t = H x t + v t Z_{t} =Hx_{t} +v_{t} Zt=Hxt+vt
注： P t − {P} _{t}^{-} Pt−为协方差矩阵 P t − = c o v ( x ^ t − , x ^ t − ) {P} _{t}^{-} =cov\left ( \hat{x} _{t}^{-},\hat{x} _{t}^{-} \right ) Pt−=cov(x^t−,x^t−) = c o v ( F x ^ t − 1 + B u t − 1 + w t , F x ^ t − 1 + B u t − 1 + w t ) =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) = F c o v ( x ^ t − 1 , x ^ t − 1 ) F T + c o v ( w t , w t ) =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) = F P t — 1 F T + Q =F{P} _{t—1}F^{T} +Q =FPt—1FT+Q
从而可以得到 Q k Q_{k} Qk与 w t w_{t} wt的关系
其中 u t − 1 u_{t-1} ut−1为常数，噪声为变量，故可用性质推的上面结果。
更新(根据观测修正预测值)
1、卡尔曼增益
K t = P t − H T ( H P t − H T + R ) − 1 K_{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 − + K t ( z t − H x ^ 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、协方差更新
P t = ( I − K t H ) P t − P_{t}=\left ( I- K_{t}H\right ) P_{t}^{-} Pt=(I−KtH)Pt−
注：其中卡尔曼增益公式可化简为(H为1,F为1)：
K t = P t − H T ( H P t − H T + R ) K_{t}=\frac{P_{t} ^{-} H^{T} }{\left ( HP_{t} ^{-}H^{T}+R \right )} Kt=(HPt−HT+R)Pt−HT
= P t − ( P t − + R ) =\frac{P_{t} ^{-}}{\left ( P_{t} ^{-}+R \right )} =(Pt−+R)Pt−
= F P t — 1 F T + Q ( F P t — 1 F T + 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
= P t — 1 + Q ( P t — 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、调节超参数( Q k Q_{k} Qk、 Z t Z_{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