车模倾角与角速度之间存在着下列等式关系：
              (4.7)
式（4.7）中， 为估计的 时刻车模倾角； 为加速度计所测得的 时刻车模倾角； 为陀螺仪所测得的 时刻角速度； 为 时刻陀螺仪的零位偏差； 为卡尔曼滤波器的采样时间。
如果令
                      (4.8)
将式（4.7）和式（4.8）写成矩阵的形式，可得到离散卡尔曼滤波的状态空间表达式如式（4.9）所示：
      (4.9)
    式（4.9）中， 和 分别为车模倾角和角速度的过程噪声。加速度计的观测方程可表示为：
                  (4.10)
    式（4.10）中， 为加速度计的观测噪声。
过程噪声的协方差矩阵 的形式如下式(4.11)所示：
                    (4.11)
观测噪声的协方差矩阵 ，且车模的倾角与角速度中含有的噪声为白噪声。
 和 分别是加速度计所测的车模倾角和陀螺仪所测的角速度的过程噪声的协方差，不同的取值代表了不同的信任程度。如果更相信来自陀螺仪的数据，可以通过设置比较小的 来将这样的信任体现到结果中去。 矩阵代表观测噪声的协方差，如果 的取值较大，那就意着加速度计测量的车模倾角中有很大的噪声[34]。
4.4  车模角度和角速度的卡尔曼滤波算法的matlab仿真
为了验证上述分析中卡尔曼滤波器模型的正确性和卡尔曼滤波算法的有效性，做以下仿真：
加速度计的测量值为： （ ）； （ ）； （ ）； （ ）。其中：加速度计的测量值和滤波后的角度值的单位均为度， 为高斯白噪声，输出噪声强度为-30dBW， 为时间（单位：秒）。陀螺仪的测量值是在真值上叠加一个输出噪声强度为-50dBW的高斯白噪声 和值为1度/秒的陀螺仪零位偏差，陀螺仪的测量值和滤波后的角速度值的单位均为度/秒。取 ， ，滤波器采样时间为0.005秒，当 ， ， 时，仿真效果比较好，如图4.5所示。
 
图4.5  卡尔曼滤波器的仿真结果
注：深蓝线代表滤波前的角度值（单位：度），红线代表滤波后的角度值（单位：度），绿线代表滤波前的角速度值（单位：度/秒），浅蓝线代表滤波后的角速度值（单位：度/秒）
图4.5表明经过0.2秒，滤波后的角速度值成功地跟随上角速度的真值，且滤波后的角度值的估计精度得到很大提高。
另外，在进行卡尔曼滤波器的matlab仿真时，根据主要参数 、 、 的变化对卡尔曼滤波器滤波效果影响的观察总结出以下规律： 变大，则滤波器开始运行后，滤波后的角速度值跟随上真值的时间变长； 变小，则滤波器开始运行后，滤波后的角速度值跟随上真值的时间变短，且滤波后的角度值变得不光滑； 变大，则滤波后的角速度值变得不光滑； 变小，则滤波后的角速度值变得更光滑； 变大，则滤波器开始运行后，滤波后的角速度值跟随上真值的时间变长，且滤波后的角速度值与真值之间有微小偏差； 变小，则滤波器开始运行后，滤波后的角速度值跟随上真值的时间变短。以上这些规律对于实际系统中卡尔曼滤波器参数的整定具有一定的指导意义。
4.5  PID算法简介及车模直立控制算法设计
4.5.1  PID算法简介
在实际工程中，应用最为广泛的调节器控制规律为比例、积分、微分控制，简称PID控制[43]。PID控制器问世至今已有近70年历史，它以其结构简单、稳定性好、工作可靠、调整方便而成为工业控制的主要技术之一。PID控制器的原理如图4.6所示。 电磁导引车控制算法研究与设计+文献综述(11):http://www.751com.cn/zidonghua/lunwen_4079.html