10 卡尔曼滤波器
10.1 线性系统的状态差分方程
系统预测状态 = 转换矩阵A * 系统上次状态 + 转换矩阵B * 系统输入 + 系统噪声w
1. 其中x是系统的状态向量，大小为n * 1 列向量 [n , 1]。
2. A为转换矩阵，大小为 n * n。
A*x_ ---> [n , n] * [n , 1] -----> [n , 1]
3. u为系统输入，大小为 k * 1。
4. B是将输入转换为状态的矩阵，大小为n * k。
B*u ---> [n , k] * [k , 1] -----> [n , 1]
5. w为系统噪声( 实际使用其协方差(多维)/方差 Q, 维度为[n, n] ), 大小为n * 1 列向量 [n , 1]。
注意这些矩阵的大小，它们与你实际编程密切相关。
10.2 看一个具体的匀加速运动的实例
有一个匀加速运动的小车，它受到的合力为 ft ,其位移速度方程如下：
0. 一个匀加速运动的小车，它受到的合力为 ft = m*a
1. 位移xt = 位移xt-1 + （速度vt + 加速度a* 时间t/2）* 时间t ；
2. 加速度a = 外力ft / 质量m ；
3. 速度vt = 速度vt-1 + 加速度a * 时间t
4. 该系统的状态向量包括位移和速度，分别用 xt 和 xt的导数 表示。
写成矩阵形式如下：
1. 其中系统状态 x 有两个二量，位移和速度，维度为 [2, 1]
2. 转换矩阵A大小为 [2, 2], 转换矩阵B大小为 [2, 1]。
3. 系统控制制输入变量为 u = f/m，也就是加速度,维度为[1, 1]，
4. 这里没有写出 系统噪声，其大小 和系统状态的维度相同，为 [2,1]
而其协方差矩阵R 维度为 [2，2] ### 10.2.2 思考
1. 貌似有了这个模型就能完全估计系统状态了，速度能计算出，位移也能计算出。
2. 那还要卡尔曼干嘛，问题是很多实际系统复杂到根本就建不了模。
3. 并且，即使你建立了较为准确的模型，只要你在某一步有误差，
4. 由递推公式，很可能不断将你的误差放大A倍（A就是那个状态转换矩阵），
以至于最后得到的估计结果完全不能用了.
既然如此，我们就引进反馈。
从 概率论 贝叶斯模型 的观点 来看 前面预测 的 结果 就是 先验，测量出的结果就是后验。
10.3 考虑测量数据 包括预测 和 实际测量值
10.3.1 利用系统状态预测的 测量数据
系统测量值的预测zk = 转换矩阵H * 系统状态预测值x + 测量噪声R的方差v > 预测的测量值是由系统状态变量映射出来的 zk = H*xk + v:
1. 其中zk是 系统测量值的预测值，大小为[m, 1](不是n * 1，也不是1 * 1，后面将说明），
2. H也是 系统状态变量 到 测量量的转换矩阵，大小为 [m, n]。
3. xK是 系统状态的预测值, 大小为 [n, 1]
H * xk -----> [m, n] * [n, 1] ---> [m, 1]
4. vk是测量过程噪声(实际使用其协方差(多维)/方差 R,维度 [m, m])，大小为[m, 1]。 #### 10.3.2 利用传感器实际测量得到的数据
同时对于匀加速模型，假设下车是匀加速远离我们，
我们站在原点用超声波仪器测量小车离我们的距离。
我们测量的是位移也就是xk，假设测量值等于xk，实际种是实际测量得到的数据
Zk = xk + 0*vk > 写成 矩阵形式：
这里 因为只对位置量做了测量，所以其噪声的维度为 1*1
10.3.2.1 思考
也就是测量值直接等于位移。
可能又会问，为什么不直接用测量值呢？
测量值噪声太大了，根本不能直接用它来进行计算。
试想一个本来是朝着一个方向做匀加速运动的小车，
你测出来的位移确是前后移动（噪声影响），只根据测量的结果，
你就以为车子一会往前开一会往后开。
对于状态方程中的 系统噪声w 和 测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。 > 噪声符合高斯分布，Q为w的协方差矩阵, R为v的协方差矩阵：
10.3.2.1 系统噪声w的协方差矩阵Q 测量噪声变量v的协方差矩阵R 实例
对于小车匀加速运动的模型，
假设系统的噪声向量只存在速度分量上，
且速度噪声的方差是一个常量0.01，
位移分量上的系统噪声为0。
只对位移进行测量，它的协方差矩阵R 大小是1*1，就是测量噪声的方差本身, > 系统噪声协方差矩阵Q 为：
1. 系统噪声协方差矩阵Q中，的主对角线上为各变量自己的方差，
2. 叠加在速度上系统噪声方差为0.01，
3. 位移上的为0，
4. 它们间协方差为0，即噪声间没有关联。
10.4 补偿系统预测值，考虑测量值的预测值和测量值之差,的到系统状态的综合估计值
系统估计值Xk = 系统状态方程预测值xk + 增益系数K * (真实测量值Zk - 测量值预测值zk)
1.理论预测（先验 Xk-1）有了，测量值（后验 测量值 Zk, 预测值zk = H*xk + v）也有了，
那怎么根据这两者得到最优的估计值呢？
2. 首先想到的就是加权(增益系数K )，或者称之为反馈。
3. 由一般的反馈思想, 设定值Zk 反馈值zk 做差 Zk - zk 之后 用比例系数K 放大， > 考虑反馈，在加上原来预测值得到估计值：
10.4.1 增益系数(反馈加权值，比例系数) K 的求解 (推导略，详情见上文给出的链接)
1. 列出 系统估计值的 协方差矩阵 Pk-1 的公式
2. 求取 Pk-1 对角线 的和 （矩阵的迹，方差的和），求导等于0，使得估计值和真实值误差最小
3. 得到 增益 K = pk * H转置 * (H * pk * H转置 + R)逆
pk = A * Pk-1 * A转置 + Q
R为测量噪声的协方差矩阵
10.5 由协方差矩阵的传播公式，得到 系统预测值 的协方差矩阵 pk
1. 系统 预测值(由历史数据的到)Xk' = A * Xk + Q ; Q为系统噪声的协方差
2. 上次 系统状态估计值 和 系统状态真实值间误差 的 协方差矩阵 Pk
3. 由协方差 的传递公式：
变量组X的线性变换， f(X) = AX，
假设X的协方差矩阵为C
则f(X)的协方差为 A * C * A转置 =
(A转置 * C逆 * A)逆 = (A转置 * C逆 * A)伪逆
4. xk = A * Xk-1 + w,
上次系统状态估计值 Xk-1 的协方差矩阵为Pk-1，系统噪声 w 的协方差矩阵为Q
得到本次 系统预测值Xk 的协方差矩阵
pk = A * Pk-1 * A转置 + Q
(相当于乘以系数的平方， 方差为 误差的平方, (x-均值)^2 , x放大了a倍，则(x-均值)^2放大a^2)
这里A 为矩阵，所以平方形式为 A * A转置 > pk = A * Pk-1 * A转置 + Q
10.6 系统估计值的 协方差矩阵 Pk
1. 系统状态 最后的估计值 XK = xk + K * （实际测量值Zk - 预测测量值zk） ， 反馈思想，见上文
其中，xk为系统预测值，
xk = A * Xk-1 + w (系统状态 过程传递),
zk = H * xk + v,
H，是状态变量到测量量的转换矩阵，
ZK 为实际测量数据。
2. 系统预测值 xk 的协方差矩阵 为 pk = A * Pk-1 * A转置 + Q
3. 系统估计值 Xk 的 协方差矩阵 Pk = pk - K * H * pk = ( I - K * H) * pk
式中，K 为 系统增益系数(反馈加权值，比例系数)
K = pk * H转置 * (H * pk * H转置 + R)逆
10.7 最后总结下递推的过程，理一下思路：
【1】首先要计算系统状态预测值xk、系统状态预测值和系统状态真实值之间误差 的 协方差矩阵pk。
1. 系统状态预测值 xk：
xk = A * Xk-1 + B * Uk + w ;
A， 为系统状态转移矩阵 ，
Xk-1，为系统状态 上次 估计值，
B， 为系统 输入转移矩阵，
Uk，为系统输入量,
w， 为系统噪声 .
2. 系统状态预测值的协方差 pk
pk = A * Pk-1 * A转置 + Q ; 协方差矩阵传递公式的到
A， 为系统状态转移矩阵 ，
Pk-1, 为上次系统状态估计值的 协方差 ,
Q， 为系统噪声的 协方差矩阵.
【2】计算卡尔曼增益 K，再然后得到 系统状态估计值 Xk
3. 卡尔曼增益 K
K = pk * H转置 * (H* pk * H转置 + R)逆 ;
pk，是系统状态预测值的协方差，
H， 是状态变量到测量量的转换矩阵，
R， 是测量噪声协方差。
4. 系统状态估计值 Xk
Xk = xk + K * （实际测量值ZK - 测量预测值zk ）
xk，系统状态预测值，
zk，是预测测量值，
zk = H * xk + v,
v， 是测量过程噪声
H，是状态变量到测量量的转换矩阵，
ZK 为实际测量数据。
【3】 最后还要计算 系统状态估计值和真实值之间的误差协方差矩阵，为下次递推做准备
5. 系统状态估计值 的 协方差 Pk
Pk = pk - K * H * pk = ( I - K * H) * pk
K，是卡尔曼增益， 系统增益系数(反馈加权值，比例系数)，
H， 是状态变量到测量量的转换矩阵，
pk，系统状态预测值的协方差
10.8 实例 matlab 小车匀加速 程序示例：
clc
clear all
close all
% 初始化参数
delta_t = 0.1; % 采样时间
t = 0:delta_t:5; % 时间范围
N = length(t); % 时间序列的长度
sz = [2,N]; % 信号需开辟的内存空间大小 2行*N列 2:为状态向量的维数n 位移 速度
g = 10; % 加速度值
x = 1/2*g*t.^2; % 实际真实位置序列 0×t + 1/2×g×t^2
z = x + sqrt(10).*randn(1,N); % 仿真的实际测量值
% 测量时加入测量白噪声 测量噪声v
% 方差R 为 10 均值为0
Q = [0 0;0 9e-1]; % 系统噪声协方差矩阵 假设建立的模型，噪声方差叠加在速度上 大小为n*n方阵 n=状态向量的维数=2
R = 10; % 位置测量协方差估计，可以改变它来看不同效果 m*m m=z(i)的维数，一个测量值
% n*n 状态 转移 矩阵 x = x * 1 + v * delta_t + 1/2*delta_t^2 *g ; v = x * 0 + v * 1 + delta_t*g
A = [1 delta_t;0 1]; % 系统状态转移矩阵
B = [1/2*delta_t^2;delta_t]; % 系统输入转移矩阵
H = [1,0]; % m*n 系统测量转移矩阵 z = 1 * x + 0 * v + 噪声
n = size(Q); % 系统状态维度数量 n为一个1*2的向量 Q为方阵
m = size(R); % 测量变量维度数量
% 分配空间
xhat = zeros(sz); % x的后验估计 状态估计值 Xk-1
P = zeros(n); % 后验方差估计 n*n 状态估计值的协方差 Pk-1
xhatminus = zeros(sz); % x的先验估计 状态 预测值 xk
Pminus = zeros(n); % n*n 状态 预测值 协方差 pk
K = zeros(n(1),m(1));% 卡尔曼增益 n*m n为系统状态数量 m为测量变量的维数
I = eye(n); % n*n 的 单位矩阵 对角矩阵
% 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0
% P=[2 0;0 2] //系统初始方差较大 算出来的 增益 K 就大，增益K 是测量真值 和 测量预测值 误差的系数 ，所以更相信测量值
for k = 9:N % 假设车子已经运动9个delta_T了，我们才开始估计
% 时间更新过程
xhatminus(:,k) = A * xhat(:,k-1) + B*g; %%%% 第一步：求系统预测值 xk 系统噪声均值为0 协方差 为 Q %%%%
% xk = A * Xk-1 + B * Uk + w ;
Pminus = A * P * A' + Q; %%%% 第二步：求系统预测值 的 协方差矩阵 %%%%
% pk = A * Pk-1 * A转置 + Q ;
% 测量更新过程
K = Pminus * H' * inv( H * Pminus*H'+ R); %%%% 第三步：计算卡尔曼增益 K %%%%
%K = pk * H转置 * (H* pk * H转置 + R)逆 ;
xhat(:,k) = xhatminus(:,k) + K*( z(k) - H * xhatminus(:,k));%%%% 第四步：计算测量系统状态估计值 Xk %%%%
% Xk = xk + K * （实际测量值ZK - 测量预测值zk ）
% zk = H * xk + v, v这里=0
P = ( I - K * H ) * Pminus; %%%% 第五步：计算估计值协方差的 协方差Pk %%%%
% Pk = pk + K * H * pk = ( I - K * H) * pk
end
figure
plot(t,z);
hold on
plot(t,xhat(1,:),'r-')
plot(t,x(1,:),'g-');
legend('含有噪声的测量', '后验估计', '真值');
xlabel('Iteration');
