卡尔曼滤波器估计单摆的角度----自己学习记录用（1）

最新推荐文章于 2024-04-21 00:21:09 发布

JianRobSim

最新推荐文章于 2024-04-21 00:21:09 发布

阅读量1.9k

点赞数 5

本文链接：https://blog.csdn.net/weixin_39090239/article/details/115363071

版权

1、前记：（菜鸡本鸡的学习记录，大佬们可以离去了_//）

记录下官网视频中如何使用卡尔曼滤波器估计单摆的角度。如下，首先明确单摆在初始角度为theta时由于重力作用下开始摆动【忽略摩擦和空气阻力的影响？】。要得到单摆在运动过程中的角度

theta，有两种办法：

（1）通过运动模型推导来预测角度；

（2）通过传感器（旋转电位计）来测量；

但是，这两种方法得到的数据存在误差--->过程噪声和测量噪声。且偏差看成是高斯白噪声（White Gaussian Noise），于是考虑使用卡尔曼滤波器对过程噪声和观测噪声给予合理的权重来估算更准确的角度值。【即：卡尔曼增益的作用，就是分配运动模型预测的状态和传感器测量的状态之间的权重。】

2、搭建模型

（1）由下基本公式开始，即如下对应：

式1表达的是每个 Xk都可以通过一个线性随机方程估计出来。任意Xk都是其前一时刻的值与过程噪音的线性组合。大部分情况下该式没有控制信号uk项。

式2告诉我们任何测量值Zk（无法确定精确与否的测量值）都是信号值与测量噪声的线性组合。这两个分量符合高斯分布。A为状态转移矩阵，B为控制矩阵，H（C）为观测矩阵【A,B为连续状态空间下的矩阵】。如下视频部分截图。

（2）卡尔曼最重要的五个公式如下

上面五个公式中，A,B,H已知。还需要知道过程噪声协方差Q和测量噪声协方差R。这里设定Q = 1e-3;
R = 1e-4【为什么这么设置我也不是很清楚？】。那么可以根据公式搭建卡尔曼滤波迭代模型了。

（1）其中卡尔曼增益K由左边公式2的过程方差预测Pk`和观测矩阵H及测量噪声协方差R计算获得。

（2）而左边公式2中的过程协方差预测Pk`又由上一时刻更新的过程协方差Pk-1（右边公式3）和过程噪声协方差Q以及状态转态矩阵A计算得到。

（3）右边公式3的协方差更新由卡尔曼增益K和观测矩阵H以及公式2的过程协方差预测Pk`计算得到。

（4）最后的滤波结果为右边公式2，Xk为左边公式1运动模型估计值Xk`加上卡尔曼增益K与传感器测量Zk的线型组合计算值得到。

在Simulink中的模型如下：

其中：Kalman Gain如下

根据官网的整体模型修改，添加自己的卡尔曼滤波模块和一个巴特沃斯低通滤波器。如下：

Kalman Filter和My Kalman Fliter的mask参数设置如下：

运行模型之前初始化参数：

% Pendulum model
% Gravity
clc
clear
g = 9.81; % [m/s^2]
% Pendulum mass
m = 1; % [kg]
% Pendulum length
l = 0.5; % [m]

% State space representation
A = [0 1; -g/l 0];
B = [0; 1/(m*l^2)];
C = [1 0];
D = 0;

% Process noise covariance
Q = 1e-3;
% Measurement noise covariance
R = 1e-4;
% Sampling time
Ts = 0.001; % [s] 
% Mykalmanfliter----DT state space离散
Ad = A*Ts+eye(2);
Bd = B*Ts;
Cd = C;