卡尔曼滤波器（二）：Simulink卡尔曼滤波器模块使用

观看MATLAB技术讲座笔记，该技术讲座视频来自bilibili账号：MATLAB中国。

本节在Simulink中用卡尔曼滤波器来滤除传感器噪声，准确估算单摆摆角。

一、单摆模型简介

不考虑摩擦时，下图所示的单摆力学平衡方程为：
$$m{l}^2\frac{d^2\theta }{d{t}^2}+mglsin\theta =\tau$$

简单做变化为：
$$\frac{d^2\theta }{d{t}^2}+\frac{g}{l}sin\theta =\frac{1}{m{l}^2}\tau$$
当摆角$\theta$较小时，有$sin\theta \approx \theta$，系统近似为线性系统。用状态空间形式表示这个线性系统，系统输入$u=\tau$，状态$x=[\theta ,\dot{\theta }{]}^T$，测量量$y=\theta$。则有：
$$\dot{x}=Ax+Bu$$
$$y=Cx+Du$$
$$A=\left[\begin{array}{cc}0& 1\\ -\frac{g}{l}& 0\end{array}\right]$$
$$B=\left[\begin{array}{c}0\\ \frac{1}{m{l}^2}\end{array}\right]$$
$$C=\left[\begin{array}{cc}1& 0\end{array}\right]$$
$$D=0$$
使用预制的单摆模块，在这里下载模型和代码。模型如下图所示。

模型有两个输入，第一个为施加的扭矩，将其设置为0。第二个输入为过程噪声，在这里假设其仅作用于角加速度。过程噪声的协方差Q在脚本中定义。

过程噪声参数设置如下，设置原理为：已知高斯白噪声的功率等于方差，但对于输出离散信号，设置功率为方差乘以采样时间。

双击单摆模块设置其状态初始值：

设置一个较小的初始角度，使其近似为线性系统。

二、线性卡尔曼滤波器使用

单摆模块输出真实摆角，让其与模拟的测量噪声相加得到测量值。然后用控制系统工具箱的卡尔曼滤波器模块来输出最佳状态估算值。

其中，测量噪声参数设置如下：

卡尔曼滤波器参数设置如下，设置初始状态与真实初始状态略有差异，观察卡尔曼滤波器在初始状态不确定时会不会收敛。由于假设过程噪声仅作用于角加速度，故Q的设置如下。交叉协方差矩阵N表示过程噪声和测量噪声的相关性，这里为0，即不相关。

运行以下脚本来提供参数：

g = 9.81;
m = 1;
L = 0.5;

A = [0, 1; -g/L, 0];
B = [0; 1/(m*L^2)];
C = [1, 0];
D = 0;

Q = 1e-3;
R = 1e-4;
Ts = 0.01;

运行Simulink仿真：

可见，虽然初始条件与模型本身略有不同，卡尔曼滤波器在大约5s处收敛，有效滤除了测量噪声，提供了准确的状态估计值。

三、非线性卡尔曼滤波器使用

当初始摆角较大时，单摆模型无法近似为非线性系统，上述KF无法准确估算状态值。如图，设置初始摆角为90°，KF无法收敛。

此时需要采用非线性的卡尔曼滤波器，这里使用扩展卡尔曼滤波器（EKF）。从控制系统工具箱拖出扩展卡尔曼滤波器模块。

建立脚本为其指定状态转换函数和测量函数：

function x = myStateTransitionFcn(x,u)
dt = 0.01;
x = x + [x(2); -9.81/0.5*sin(x(1)) + 1/0.5^2*u]*dt;
end

function y = myMeasurementFcn(x)
y = x(1);
end

为扩展卡尔曼滤波器模块设置参数，这里也可以指定雅可比矩阵的计算函数，如果不指定则会计算数值解。设置初始估计状态的误差协方差为较小的值表示比较确定初始状态的值。如果有多个测量值则可点击“Add Measurement”添加。

连接输入，并通过零阶保持器模块来离散化信号，这是因为扩展卡尔曼滤波器模块估算的是离散非线性系统的状态：

零阶保持器参数设置如下：

运行仿真：

局部放大：

如果系统的非线性较强，EKF可能效果会变差，此时可以使用无迹卡尔曼滤波器。