References:

卡尔曼滤波，本质上是一种最优的递归的数字处理算法。卡尔曼滤波器的应用非常广泛，这是因为在实际的系统当中，存在许多的不确定性，主要包含以下三个方面：
① 不存在完美的数学模型
② 系统的扰动不可控，也很难建模
③ 测量传感器存在误差

KF

卡尔曼滤波原理介绍

假设我们要研究的对象是一个房间的温度。根据经验判断,这个房间的温度大概在25℃左右,可能受空气流通、阳光等因素影响,房间内温度会小幅度地波动。我们以分钟为单位,定时测量房间温度,这里的1分钟,可以理解为采样时间。假设测量温度时,外界的天气是多云,阳光照射时有时无,同时房间不是100%密封的,可能有微小的与外界空气的交换,即引入过程噪声$W(k)$,其方差为$Q$,大小假定为$Q=0.01$(假如不考虑过程噪声的影响,即真实温度是恒定的,那么这时候$Q=0$)。相应地, $A=1$, $F=1$, $Q=0.01$, 状态X(k)是在第k分钟时的房间温度,是一维的。那么该系统的状态方程可以写为: $X(k)=X(k-1)+W(k)$

现在用温度计开始测量房间的温度,假设温度计的测量误差为±0.5℃,从出厂说明书上我们得知该温度计的方差为0.25。也就是说,温度计第k次测量的数据不是100%准确的,它是有测量噪声V(k)的,并且其方差$R=0.25$,因此测量方程为$Z(k)=X(k)+V(k)$。
该系统的状态和观测方程为

$X(k)=AX(k-1)+W(k-1)$
$Z(k)=HX(k)+V(k)$

式中,$X(k)$是一维变量温度; $A=1$; $F=1$; $H=1$; $W(k)$和$V(k)$的方差为$Q$和$R$。

模型建好以后,就可以利用 Kalman滤波了。假如要估算第k时刻的实际温度值,首先要根据第$k-1$时刻的温度值来预测k时刻的温度。
(1)假定第k-1时刻的温度值测量值为23.9℃,房间真实温度为24.0℃,该
测量值的偏差是0.1℃,即协方差$P(k-1)=0.1^2$。
(2)在第k时刻,房间的真实温度是24.1℃,温度计在该时刻测量的值为
24.5℃,偏差为0.4℃。我们用于估算第k时刻的温度有两个温度值,分别是k-1时刻23.9℃和k时刻的24.5℃,如何融合这两组数据,得到最逼近真实值的估计?
首先,利用k1时刻温度值预测第k时刻的温度,其预计偏差为$P(k|k-1)=P(k-1)+Q=0.02$,计算 Kalman增益
$K=P(k|k-1)/(P(k|k-1)+R)=00741$,那么这时候利用$k$时刻的观测值,得到温度的估计值为$X(k)=23.9+0.0741×(24.1-23.9)=23.915℃$。可见,与23.9℃和24.5℃相比较, Kalman估计值23.915℃更接近真实值24.1℃。此时更新k时刻的偏差$P(k)=(1-K*H)P(k|k-1)=0.0186$。最后由$X(k)=23.915℃$和$P(k)=0.0186$,可以继续对下一时刻观测数据$Z(k+1)$进行更新和处理。
(3)这样, Kalman滤波器就不断地把方差递归，从而估算出最优的温度值。$X(0)$和$P(0)$分别为滤波器初始值。

MATLAB仿真程序

% 程序说明：Kalman滤波用于一维温度测量的实例
function Kalman_main
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=120;%采样点个数，时间单位为分钟
CON=25;%室内温度理论值，房间温度在25摄氏度左右

%对状态和测量初始化
Xexpect=CON*ones(1,N);%期望温度是25摄氏度，但会收到噪声影响
X=zeros(1,N);  %房间各时刻真实温度值
Xkf=zeros(1,N); %估计值
Z=zeros(1,N);  %温度计测量值
P=zeros(1,N); 

%初始化
X(1)=25.1;
P(1)=0.01;%初始化协方差
Z(1)=24.9;
Xkf(1)=Z(1);%初始化测量值24.9，可作为滤波器的初始估计状态

%噪声
Q=0.01;%W(k)的方差
R=0.25;%V(k)的方差
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);

%系统矩阵
F=1;
G=1;
H=1;
I=eye(1); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%模拟房间温度和测量过程，并滤波
for k=2:N
    %第一步：随时间推移，房间真实温度波动变化
    X(k)=F*X(k-1)+G*W(k-1);  %状态方程
    %第二步：随时间推移，获取实时数据
    Z(k)=H*X(k)+V(k); %观测方程
    %第三步：Kalman滤波
    X_pre=F*Xkf(k-1);  %状态估计         
    P_pre=F*P(k-1)*F'+Q; %协方差预测       
    Kg=P_pre*inv(H*P_pre*H'+R); %kalman增益
    e=Z(k)-H*X_pre;      %新息      
    Xkf(k)=X_pre+Kg*e;   %状态更新 
    P(k)=(I-Kg*H)*P_pre; %协方差更新
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算误差
Err_Messure=zeros(1,N);%量测值与真实值的误差
Err_Kalman=zeros(1,N);%估计与真实值的偏差
for k=1:N
    Err_Messure(k)=abs(Z(k)-X(k));
    Err_Kalman(k)=abs(Xkf(k)-X(k));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
%一次画出理论值、真实值、测量值、估计值
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-k',t,Xkf,'-g');
legend('expected','real','measure','kalman extimate');         
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');
%误差分析
figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');
legend('messure error','kalman error');         
xlabel('sample time');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

kalman滤波算法特点

　　（1）由于kalman滤波算法是将被估计信号看作在白噪声作用下一个随机线性系统的输出，并且它的输入/输出是由状态方程和输出方程在时间域上给出的，因此这种滤波方法不仅适用于平稳随机过程的滤波，而且特别适用于非平稳或平稳马尔科夫序列或高斯-马尔科夫序列的滤波，因此应用范围非常广泛。

　　（2）kalman滤波算法是一种时间域滤波方法，采用状态空间描述系统。系统的过程噪声和量测噪声不是需要滤除的对象，他们的统计特性正是估计过程中要用到的信息，而被估计量和观测量在不同时刻的一、二阶矩是不需要知道的。

　　（3）由于kalman滤波的基本方程是时间域内的递推形式，其计算过程是一个不断“预测-修正”的过程，在求解时不需要存储大量数据，同时一旦观测到新的数据，便可以算得新的滤波值，因此这种滤波方法非常适用于实时处理、计算机实现。

　　（4）由于滤波器的增益矩阵与观测无关，因此它可预先离线算出，从而可以减少实时计算量。求滤波器增益矩阵时，要求一个矩阵的逆，它的阶数只是取决于观测方程的维数，而该维数通常很小，因此求逆运算是比较方便的。另外，在求解滤波器增益的过程中，随时可以算得滤波器的精度指标P，其对角线上的元素就是滤波误差向量各个分量的方差。

EKF

　　kalman滤波能够在线性高斯模型的条件下，对目标的状态做出最优的估计，可以有一个较好的跟踪效果。但是，在实际应用中往往会有很多不同程度的非线性，其中非线性函数比较典型的几个分别是平方、对数、指数、三角函数等，有些非线性系统可以近似看成线性系统，但为了精确估计系统的状态，大多数系统不能仅用线性微分来描述。例如飞机的飞行、导弹的制导等，其中的非线性因素不能忽略，必须建立适用于非线性系统的滤波算法。

　　对于非线性系统的滤波问题，最常用的方法是利用现行换技巧将其转化为一个近似的线性滤波问题，其中最广泛的方法是扩展kalman滤波方法（Extended kalman Filter , EKF）。扩展kalman滤波建立在线性kalman滤波的基础上，它的核心思想就是，对与一般的非线性系统，首先围绕滤波值将非线性函数f()和h()展开成泰勒级数，然后省略二阶及以上的项，得到一个近似的线性化模型，然后再用kalman滤波完成对目标的滤波估计等处理。

　　EKF的优点在于不用预先计算过程噪声W(k)和量测噪声V(k)均为零的时候的解，但它只能在滤波误差以及一步预测误差比较小的时候才能用。

EKF算法步骤

UKF

无迹卡尔曼滤波（UKF）

　　第二部分讨论的扩展kalman滤波算法是对非线性的系统方程或者观测方程进行泰勒展开并保留其中一阶近似项，这样不可避免地引入了线性化误差。如果线性化假设不成立，采用这种算法则会导致滤波器性能下降以至于造成发散。如果线性化假设不成立，采用这种算法则会导致滤波器性能下降以至于造成发散。另外，在一般情况下计算系统状态方程和观测方程的jacobian矩阵是不易实现的，增加了算法的计算复杂度。

　　无迹卡尔曼滤波（Unscented Kalman Filter，UKF）摒弃了对非线性函数进行线性化的传统做法，采用kalman线性滤波框架，对用于预测方程，使用无迹变换（Unscented Transform,UT）来处理均值和协方差的非线性传递问题。UKF算法是对非线性函数的概率密度分布进行了近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要对雅可比矩阵进行求导。同时，UKF没有把高阶项忽略，因此对于非线性分布的统计量有较高的计算精度，有效地克服了EKF的估计精度低、稳定性差的问题。

卡尔曼滤波需要线性模型，需要通过泰勒展开来完成局部线性化便可以实现 EKF，但是是否存在线性化的更好的方法，在文中，我们将引入无迹变换，随后探讨无迹卡尔曼滤波器。

泰勒展开是通过泰勒展开对非线性函数进行线性化
无迹变换是计算一系列点（关键点，Sigma Point），然后通过非线性函数进行变换，通过变换结果和对应的权重来计算高斯分布

接下来我们需要考虑两个问题：

如何选择Sigma 点
如何设置对应的权重

需要满足以下条件，但是满足以下条件的解很多。

$\begin{aligned} &\sum^iw^{[i]}=1 \\ &\mu=\sum_{i}w^{[i]}\mathcal{X}^{[i]} \\ &\Sigma=\sum_{i}w^{[i]}(\mathcal{X}^{[i]}-\mu)(\mathcal{X}^{[i]}-\mu)^{T} \end{aligned}$

可以根据经验，我们按照以下规则选择 Sigma 点

$\begin{aligned} &\chi[0] =\mu \\ &\chi[i] =\mu+\left(\sqrt{(n+\lambda)\Sigma}\right)_i\quad for i= 1,\ldots n \\ &\chi[i] =\mu-\left(\sqrt{(n+\lambda)\Sigma}\right)_{i-n}\quad for i= n+1,\ldots2n \end{aligned}$

矩阵平方根

为了求解上述Sigma 点，我们需要对矩阵进行平方根的求解，我们定义矩阵 $S$ ，满足 $Σ=S^TS$ ，按照对角化原则

$\begin{aligned} \sum& =VDV^{-1} \\ &=V\begin{pmatrix}d11&\cdots&0\\\\0&\ddots&0\\\\0&\ddots&dnn\end{pmatrix}V^{-1} \\ &=V\begin{pmatrix}\sqrt{d11}&\cdots&0\\\\0&\ddots&0\\\\0&\ddots&\sqrt{dnn}\end{pmatrix}\begin{pmatrix}\sqrt{d11}&\cdots&0\\\\0&\ddots&0\\\\0&\ddots&\sqrt{dnn}\end{pmatrix}V^{-1} \\& \end{aligned}$ $S=V\underbrace{\left(\begin{array}{ccc}{\sqrt{d11}}&{\cdots}&{0}\\{0}&{\ddots}&{0}\\{0}&{\ddots}&{\sqrt{dnn}}\end{array}\right)}_{D^{1/2}}V^{-1}$

Cholesky分解

定义下三角矩阵 $L $满足 $Σ=LL^T$ ，该方法能够提供稳定的数值解，经常在UKF应用中使用， $L$ 和 $Σ$ 拥有相同的特征值。Sigma 点可以但不必须位于协方差的主轴上。

Sigma点权重

$w_{m}^{[i]}$ 用于计算均值， $w_{c}^{[i]}$ 用于计算协方差

$\begin{aligned}&w_{m}^{[0]}=\frac\lambda{n+\lambda}\\&w_{c}^{[0]}=w_m^{[0]}+(1-\alpha^2+\beta)\\&w_{m}^{[i]}=w_c^{[i]}=\frac\lambda{2(n+\lambda)} for i=1,\ldots,2n\end{aligned}$

接下来，对计算出来的Sigma 点的映射值，做高斯估计

$\begin{aligned}&\mu^{\prime}=\sum_iw_m^{[i]}\mathcal{X}^{[i]}\\&\Sigma^{\prime}=\sum_{i=0}^{2n}w_c^{[i]}(\mathcal{X}^{[i]}-\mu^{\prime})(\mathcal{X}^{[i]}-\mu^{\prime})^T\end{aligned}$

UKF Algorithm - Prediction

$\text{Unscented Kalman filter} (\mu_{t-1},\Sigma_{t-1},u_t,z_t){:}$
$\mathcal{X}_{t-1}=(\mu_{t-1}\quad\mu_{t-1}+\sqrt{(n+\lambda)\Sigma_{t-1}}\quad\mu_{t-1}-\sqrt{(n+\lambda)\Sigma_{t-1}})$
$\bar{\mathcal{X}}_{t}^{*}=g (u_{t},\mathcal{X}_{t-1})$
$\bar{\mu}_{t}=\sum_{i=0}^{2n}w_{m}^{[i]}\bar{\mathcal X}_{t}^{*[i]}$
$\bar{\Sigma}_{t}=\sum_{i=0}^{2n}w_{c}^{[i]}(\bar{\mathcal{X}}_{t}^{\ast[i]}-\bar{\mu}_{t})(\bar{\mathcal{X}}_{t}^{\ast[i]}-\bar{\mu}_{t})^{T}+R_{t}$
$\bar{\mathcal{X}}_{t}=(\bar{\mu}_{t}\quad\bar{\mu}_{t}+\sqrt{(n+\lambda)\bar{\Sigma}_{t}}\quad\bar{\mu}_{t}-\sqrt{(n+\lambda)\bar{\Sigma}_{t}})$
$\bar{\mathcal Z}_{t}=h (\bar{\mathcal X}_{t})$
$\hat{z}_{t}=\sum_{i=0}^{2n}w_{m}^{[i]}\bar{\mathcal Z}_{t}^{[i]}$
$S_{t}=\sum_{i=0}^{2n}w_{c}^{[i]}(\bar{\mathcal{Z}}_{t}^{[i]}-\hat{z}_{t})(\bar{\mathcal{Z}}_{t}^{[i]}-\hat{z}_{t})^{T}+Q_{t}$
$\bar{\Sigma}_{t}^{x, z}=\sum_{i=0}^{2n}w_{c}^{[i]}(\bar{\mathcal X}_{t}^{[i]}-\bar{\mu}_{t})(\bar{\mathcal Z}_{t}^{[i]}-\hat{z}_{t})^{T}$
$K_{t}=\bar{\Sigma}_{t}^{x, z} S_{t}^{-1}$
$\mu_{t}=\bar{\mu}_{t}+K_{t}(z_{t}-\hat{z}_{t})$
$\Sigma_{t}=\bar{\Sigma}_{t}-K_{t} S_{t} K_{t}^{T}$
$\text{return} ~ \mu_{t},\Sigma_{t}$

$\begin{aligned} \Sigma_{t}& =\left(I-K_{t}H_{t}\right)\bar{\Sigma}_{t} \\ &=\bar{\Sigma}_t-K_tH_t\bar{\Sigma}_t \\ &=\bar{\Sigma}_t-K_t\left(\bar{\Sigma}^{x,z}\right)^T \\ &=\bar{\Sigma}_t-K_t\left(\bar{\Sigma}^{x,z}S_t^{-1}S_t\right)^T \\ &=\bar{\Sigma}_t-K_t(K_tS_t)^T \\ &=\bar{\Sigma}_t-K_tS_t^TK_t^T \\ &=\bar{\Sigma}_t-K_tS_tK_t^T \end{aligned}$

无迹变换是线性化的另一种方式
无迹变换的拟合效果比泰勒分布更好
无迹变换使用的是Sigma点传播
无迹变换存在自由参数
无迹卡尔曼滤波在预测和更新步使用无迹变换

UKF vs. EKF

如果是线性模型，则两者结果相同
如果是非线性模型，则UKF结果比EKF好
两者差异很小
UKF不需要计算雅可比
复杂度在同一级别
UKF比EKF稍慢
依然受限于高斯分布

参考：

一手资料：无迹变换

https://zhuanlan.zhihu.com/p/399370551

无迹卡尔曼滤波UKF的理解与应用（附Matlab实例）

TEST:

$\lim_{h \rightarrow 0 } \frac{f(x+h)-f(x)}{h}$

IMM

　　在kalman滤波算法中用到了状态转移方程和量测方程，被估计量随着时间的变化，呈现的是一个动态估计。在目标跟踪中，不需要知道目标的运动模型就能实时的修正目标的状态变量（速度、距离等），具有良好的适应性。但是当目标实施机动变化（突然加、减速或急转弯等），仅仅采用基本的kalman滤波算法往往得不到理想的结果。这时就需要采用自适应算法。交互多模型（IMM）就应用而生。

　　目标交互多模型kalman滤波算法在机动目标跟踪领域得到广泛应用。IMM算法使用两个或者多个模型来描述工作过程中可能出现的状态，最后通过有效的加权融合进行系统状态估计，很好的克服了单个模型估计误差较大的问题。

PSINS工具箱卡尔曼滤波器

(卡尔曼滤波算法综述（KF、EKF、UKF和IMM）)[https://www.cnblogs.com/sbb-first-blog/p/17178998.html]

https://www.cnblogs.com/milton/p/18038841

四、优秀解释
无人驾驶技术入门（十八）| 手把手教你写扩展卡尔曼滤波器：https://zhuanlan.zhihu.com/p/63641680

动手算一算温度

https://zhuanlan.zhihu.com/p/93011093

卡尔曼滤波示例

https://zhuanlan.zhihu.com/p/29191795

卡尔曼滤波：从入门到精通

https://zhuanlan.zhihu.com/p/36745755

卡尔曼滤波中关键参数的调整