观看MATLAB技术讲座笔记,该技术讲座视频来自bilibili账号:MATLAB中国。
本节在Simulink中用卡尔曼滤波器来滤除传感器噪声,准确估算单摆摆角。
一、单摆模型简介
不考虑摩擦时,下图所示的单摆力学平衡方程为:
m l 2 d 2 θ d t 2 + m g l s i n θ = τ ml^2\frac{d^2\theta}{dt^2}+mglsin\theta=\tau ml2dt2d2θ+mglsinθ=τ
简单做变化为:
d 2 θ d t 2 + g l s i n θ = 1 m l 2 τ \frac{d^2\theta}{dt^2}+\frac{g}{l}sin\theta=\frac{1}{ml^2}\tau dt2d2θ+lgsinθ=ml21τ
当摆角 θ \theta θ较小时,有 s i n θ ≈ θ sin\theta\approx\theta sinθ≈θ,系统近似为线性系统。用状态空间形式表示这个线性系统,系统输入 u = τ u=\tau u=τ,状态 x = [ θ , θ ˙ ] T x=[\theta,\dot{\theta}]^T x=[θ,θ˙]T,测量量 y = θ y=\theta y=θ。则有:
x ˙ = A x + B u \dot x=Ax+Bu x˙=Ax+Bu
y = C x + D u y=Cx+Du y=Cx+Du
A = [ 0 1 − g l 0 ] A=\begin{bmatrix} 0 & 1 \\ -\frac{g}{l} & 0 \\ \end{bmatrix} A=[0−lg10]
B = [ 0 1 m l 2 ] B=\begin{bmatrix} 0\\ \frac{1}{ml^2}\\ \end{bmatrix} B=[0ml21]
C = [ 1 0 ] C=\begin{bmatrix} 1 & 0\\ \end{bmatrix} C=[10]
D = 0 D=0 D=0
使用预制的单摆模块,在这里下载模型和代码。模型如下图所示。
模型有两个输入,第一个为施加的扭矩,将其设置为0。第二个输入为过程噪声,在这里假设其仅作用于角加速度。过程噪声的协方差Q在脚本中定义。
过程噪声参数设置如下:
双击单摆模块设置其状态初始值:
设置一个较小的初始角度,使其近似为线性系统。
二、线性卡尔曼滤波器使用
单摆模块输出真实摆角,让其与测量噪声相加得到测量值。然后用控制系统工具箱的卡尔曼滤波器模块来输出最佳状态估算值。
其中,测量噪声参数设置如下:
卡尔曼滤波器参数设置如下,设置初始状态与真实初始状态略有差异,观察卡尔曼滤波器在初始状态不确定时会不会收敛。由于假设过程噪声仅作用于角加速度,故Q的设置如下。交叉协方差矩阵N表示过程噪声和测量噪声的相关性,这里为0,即不相关。
运行以下脚本来提供参数:
matlab">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无法准确估算状态值,需要使用非线性卡尔曼滤波器…待续