观看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无法准确估算状态值，需要使用非线性卡尔曼滤波器…待续