暑假期间，对于四旋翼有一点兴趣，没有亲手做，但是看了一些资料。这个系列文章只是对自己看的东西的记录，对于想要学习了解相关知识的同学没有任何参考价值！
本篇是系列的第二部分：介绍了我自己对于串级PID和卡尔曼滤波的理解。
1.PID的思想
PID可以说是最著名的控制方式，公式如下（离散）
y
(
n
)
=
K
P
e
(
n
)
+
K
i
∑
i
=
0
n
e
(
i
)
+
K
d
[
e
(
n
)
−
e
(
n
−
1
)
]
y(n)=K_Pe(n)+K_i\sum_{i=0}^ne(i)+K_d[e(n)-e(n-1)]
y(n)=KPe(n)+Kii=0∑ne(i)+Kd[e(n)−e(n−1)]
y(n)是输出量，e(n)是目标量与输出量之间的误差。PID就是一个输出量和误差之间的函数。
输出量由这么几个部分组成：
1.比例项：以KP为系数，作为当前的误差对于输出的影响，如果误差越大，就要加大输出；
2.积分项：以KI为系数，作为过去误差对于输出的影响，如果累积的误差越大，输出也要考虑；
3.微分项：以KD为系数，作为将来误差对于输出的影响，如果误差变化率越大，我们预计将来的误差也会越大，所以输出也要考虑。
调节参数的影响：
1.KP越大，则被控质量追踪目标量的速度越快，但是被控质量在目标量附近震荡越大；
2.KD越大，则被控质量变换速度减缓，优点在于震荡也会变小，KD有压制KP的作用
3.KI的作用是消除稳差，但是参数不合适也会有累积效应。
2.串级PID
以我参考的开源四旋翼代码为例讲一讲我的理解：
在控制姿态角度时，使用的就是两级PID串联，第一级输入量是当前实际的偏航角度，目标量是遥控器发送的目标角度。也就是使用外环PID来控制角度，输出量自然就是角速度。
但是，飞行器的主控芯片实际能够控制的，只是桨叶的转速大小，而桨叶转速和角速度之间的关系是不确定的。所以就需要增加一个内环，目标量是外环的输出量，输入为当前的角速度，输出是发送给电调的信号。如果桨叶转速和角速度之间有一个明确的公式，那么一层PID就足够了，但是事实是不行。
外环PID一般不加入积分微分，只有比例P。因为添加I后相应变慢，添加D又会引入噪声。
卡尔曼滤波似乎是一个可以很复杂也可以很简单的东西，下面说一下我自己的理解。举个例子：我们一辆以1m/s匀速行驶的小车，开始离墙10m，1s后离墙距离，按照计算是9m。但是传感器的数据是8.9m，那么我应该信任哪个数据呢？合理的方式是结合两个数据，算出概率期望最大的数值。这个过程就是卡尔曼滤波。
1.运动方程和观测方程
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
(
1
)
z
k
=
h
(
x
k
)
+
v
k
(
2
)
x_k=f(x_{k-1},u_k)+w_k (1)\\ z_k=h(x_k)+v_k(2)
xk=f(xk−1,uk)+wk(1)zk=h(xk)+vk(2)
公式1是运动方程，xk是当前的状态，xk-1是上一个时刻的状态，uk是输入量，wk是噪声。
公式2是观测方程，xk是当前的状态量，zk是观测量，vk是传感器所加入的噪声。
这里要再假设两点：1.马尔可夫性：k时刻的状态只与k-1时刻有关。2.状态xk的概率服从高斯分布。
2.线性系统与卡尔曼滤波
假设状态方程与观测方程都是线性的
x
k
=
A
k
x
k
−
1
+
u
k
+
w
k
z
k
=
C
k
x
k
+
v
k
x_k=A_kx_{k-1}+u_k+w_k\\z_k=C_kx_k+v_k
xk=Akxk−1+uk+wkzk=Ckxk+vk
并且假设wk和vk都服从高斯分布，均值为0，方差分别为R和Q。
第一步：确定先验状态的两个方程，由于假定了高斯分布，根据高斯分布的叠加性质，可以求得
P
(
x
k
∣
x
k
−
1
,
u
1
:
k
,
z
1
:
k
−
1
)
∼
N
(
A
k
x
k
−
1
^
+
u
k
,
A
k
P
k
−
1
^
A
K
T
+
R
)
P(x_k|x_{k-1},u_{1:k},z_{1:k-1})\sim N(A_k\hat{x_{k-1}}+u_k,A_k\hat{P_{k-1}}A_K^T+R)
P(xk∣xk−1,u1:k,z1:k−1)∼N(Akxk−1^+uk,AkPk−1^AKT+R)
两个先验方程也就得到了
x
k
‾
=
A
k
x
k
−
1
^
+
u
k
P
k
‾
=
A
k
P
k
−
1
^
A
K
T
+
R
\overline{x_k}=A_k\hat{x_{k-1}}+u_k\\\overline{P_k}=A_k\hat{P_{k-1}}A_K^T+R
xk=Akxk−1^+ukPk=AkPk−1^AKT+R
更新方程一共有三个，首先是计算K值卡尔曼增益
K
=
P
k
‾
C
K
T
(
C
k
P
k
‾
C
K
T
+
Q
k
)
−
1
K=\overline{P_k}C_K^T(C_k\overline{P_k}C_K^T+Q_k)^{-1}
K=PkCKT(CkPkCKT+Qk)−1
然后计算后验概率的分布
x
k
^
=
x
k
‾
+
K
(
z
k
−
C
k
x
k
‾
)
P
k
^
=
(
I
−
K
C
k
)
P
k
‾
\hat{x_k}=\overline{x_k}+K(z_k-C_k\overline{x_k})\\\hat{P_k}=(I-KC_k)\overline{P_k}
xk^=xk+K(zk−Ckxk)Pk^=(I−KCk)Pk
有很多资料Q和R的定义是反过来的，这样最后的公式也要相应调整。卡尔曼滤波可以从概率上证明是这个系统的最优解。
3.非线性系统到EKF
EKF也就是扩展卡尔曼滤波，对于非线性系统，通常做法是在某个点附近考虑运动方程以及观测方程的一阶泰勒展开。实际运用中，由于矩阵求导太复杂，我感觉我一时半会用不到，这个问题留到以后用到了再说。
4.一个使用卡尔曼滤波的实例：气压计和加速度融合定高
在网上看到一个教学项目，搬到这里来
这里使用MPU6050和DPS130卡尔曼滤波融合定高，首先对系统建模。
用x(k)代表系统在z轴上的状态，那么状态应该包含三个量：高度，速度，加速度。
x
(
k
)
=
[
h
(
k
)
v
(
k
)
a
(
k
)
]
x(k)=\begin{bmatrix} h(k)\\v(k)\\a(k) \end{bmatrix}
x(k)=⎣⎡h(k)v(k)a(k)⎦⎤
假设采样周期非常短，可以认为a(k+1)=a(k)
求得状态转移方程
[
h
(
k
+
1
)
v
(
k
+
1
)
a
(
k
+
1
)
]
=
[
1
T
0.5
T
2
0
1
T
0
0
1
]
[
h
(
k
)
v
(
k
)
a
(
k
)
]
\begin{bmatrix} h(k+1)\\v(k+1)\\a(k+1) \end{bmatrix}=\begin{bmatrix} 1&&T&&0.5T^2\\0&&1&&T\\0&&0&&1 \end{bmatrix} \begin{bmatrix} h(k)\\v(k)\\a(k) \end{bmatrix}
⎣⎡h(k+1)v(k+1)a(k+1)⎦⎤=⎣⎡100T100.5T2T1⎦⎤⎣⎡h(k)v(k)a(k)⎦⎤
观测量y(k)包括了加速度和气压两个量，所以
y
(
k
)
=
[
P
(
k
)
a
(
k
)
]
y(k)=\begin{bmatrix} P(k)\\a(k) \end{bmatrix}
y(k)=[P(k)a(k)]
根据气压与高度的线性变化关系，可以求得：
[
P
(
k
)
a
(
k
)
]
=
[
−
1
/
0.09
0
0
0
0
1
]
[
h
(
k
)
v
(
k
)
a
(
k
)
]
+
[
P
0
0
]
\begin{bmatrix} P(k)\\a(k) \end{bmatrix}=\begin{bmatrix} -1/0.09&&0&&0\\0&&0&&1 \end{bmatrix} \begin{bmatrix} h(k)\\v(k)\\a(k) \end{bmatrix} +\begin{bmatrix} P_0\\0 \end{bmatrix}
[P(k)a(k)]=[−1/0.0900001]⎣⎡h(k)v(k)a(k)⎦⎤+[P00]
这就是观测方程，然后可以开始写卡尔曼滤波代码
这里设定测量误差R和过程误差Q，和上面的推导是反过来的。测量误差R是反映传感器得到信息质量的优劣，传感器得到信号质量越差，则R应越大。Q是过程噪声，如果为0，意味着将预测值当成最终结果。测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。
代码如下
clear;
load('DataUp.mat');
len = length(z);
% 减去重力加速度后的加速度
a = (z-1)*9.8;
% 参数设置
sam_frq = 1000;
T = 1/sam_frq;
k = 0.09;%压高系数
% 观测误差R、过程误差Q
R = 0.5*eye(2);
R(1,1) = 150;
R(2,2) = 0.5;
Q = 0.0001*eye(3);
Q(2,2) = 0.00001;
% 状态方程矩阵
F = [1,T,0.5*T*T;0,1,T;0,0,1];
H = [-1/k,0,0;0,0,1];
V = [pre(1);0];
% 数据初始化
Xkf = zeros(3,len);
Z = zeros(2,len);
Z(1,:) = pre;
Z(2,:) = a;
Xkf(:,1) = [0;0;0];
P0 = eye(3);
P0(1,1) = 10;
% 卡尔曼滤波
for i = 2:len
Xn = F*Xkf(:,i-1);
P1 = F*P0*F'+ Q;
K = (P1*H')/(H*P1*H'+R);
Xkf(:,i) = Xn + K*(Z(:,i)-H*Xn-V);
P0 = (eye(3)-K*H)*P1;
end
%显示图像
figure;
plot((pre(1)-pre)*0.09);
hold on;
plot(Xkf(1,:));
hold on;
plot(Xkf(2,:));
hold on;
plot(Xkf(3,:));
hold on;
plot(a);在这里插入代码片
读取的MAT文件中共有73655组数据
上图是气压计直接求得高度数据和卡尔曼滤波后的高度数据比较，两者似乎很接近。
相应的速度和加速度经过卡尔曼滤波后的数据也能求出来。
上图是测量加速度和卡尔曼滤波后加速度对比，绿色滤波前，红色滤波后。