卡尔曼滤波

概念：

滤波： 信号x 权值 + 噪声x权值
卡尔曼滤波： 最优估计值x 权值 + 观测值 x 权值
卡尔曼滤波用上一次的最优结果预测当前值，同时使用观测值修正当前值，得到最优的结果。
适用: 线性高斯系统
线性: 不是线性用EKF 即不是线性(叠加性与齐次性)化为线性再进行卡尔曼滤波
高斯: 噪声满足正态分布

基础表达式：

状态方程： xk = A *xk-1 + B *uk + ωk;
xk-1 ： 代表上一个时刻状态；
uk ： 输入；
wk ： 过程噪声；（真实物理系统）
A: 状态转移矩阵；
B： 控制矩阵；
观测方程： yk = C *xk + νk;

xk： 状态量
vk: 观测噪声与观测传感器有关；
状态方程与观测方程联系：

参数分析

调参： Qk（过程噪声标准差）、Rk（观测噪声标准差）
wk和vk都应该服从高斯分布-高斯白噪声；
Rk： 传感器测量值比如为 1000 ± δ 则 vk为δ；δ方差为Rk 则vk方差为Rk vk∈N（0：Rk的标准差）
Qk： 环境过程噪声比如风、摩擦 直接服从正态分布 wk ∈ N（0;Qk的标准差);
估计值(状态)值： 服从正态分布：
一维为：xk^-
二维为：

修正值(卡尔曼滤波输出)后验估计值xk-1^
基于修正值——到 先验估计值xk^- + 观测值yk=xk(两个高斯分布融合)得到最优估计值xk^

公式计算：

预测：

1.先验估计

xt^- = F xt-1^ + B ut-1 (F状态转移矩阵 B控制量)**

2.先验估计协方差
Pt^- = F Pt-1 F转置 + Q(wk过程噪声的协方差);**

外部测量：

3.观测 zt = H *xt + v(观测噪声);

更新：

4.修正估计(卡尔曼滤波最后结果)
xt^ = xt^- + Kt (Zt - Hxt^-); xt^先验估计值 Kt卡尔曼增益
Zt为观测值 H 为矩阵；

5.更新卡尔曼增益(与Q和R都有关系)

Kt = （Pt^- * H转置） / （ H * Pt^- *H转置 + R）；（R为观测噪声的方差）

6.更新后验估计协方差（修正估计协方差、利用观测）
Pt = （I - Kt*H）Pt^-;

卡尔曼滤波器超参数调节Q 和 R

K = （Pt-1 + Q） / (Pt-1 + Q + R)
如果应该更多信任观测值(传感器精度高)则 R 越小 K越大；
如果应该更多信任先验估计即过程噪声无限小则调整 R越大 Q增大 K减小；
Q过程噪声方差： 理想情况 无过程噪声 Q无穷小；
R观测噪声方差： 传感器精度高 R小；

卡尔曼滤波器参数调节P0 和 X0^

P0:Pt初值习惯取小,方便收敛（Pt一般为1）不能为0
X0：xt^最开始取0；
一维无控制滤波 ：F =1 ; H =1 ;