扩展卡尔曼滤波EKF的公式及Matlab实现

EKF简介

首先kalman滤波器的作用是在包含不确定性的动态系统中做最优估计，滤波也就是估计的意思。但是kalman滤波器只能对线性系统进行估计，它要求状态转移方程和观测方程都是线性的，具体就是，满足如下的状态转移方程：

$$x_k=F_k\ast x_{k-1}+B_k\ast u_k+w_k$$

其中，

• $x_k$ 是当前时间步的状态向量（待估计的状态）。
• $F_k$ 是状态转移矩阵，表示状态如何从上一时刻转移到当前时刻。
• $x_{k-1}$ 是上一时刻的状态向量。
• $B_k$ 是控制输入矩阵，用于表示外部控制对系统状态的影响（在某些情况下可能不存在）。
• $u_k$ 是当前时间步的控制输入向量。
• $w_k$ 是过程噪声，假设为高斯白噪声，表示状态转移过程中的不确定性或随机扰动。

满足如下的观测方程：

$$z_k=H_k\ast x_k+v_k$$

其中，

• $z_k$ 是当前时间步的观测向量（通过传感器等获取的实际测量值）。
• $H_k$ 是观测矩阵，用于将状态映射到观测空间。
• $x_k$ 是当前时间步的状态向量。
• $v_k$ 是观测噪声，假设为高斯白噪声，表示观测过程中的不确定性或随机误差。

而EKF满足的状态转移方程和观测方程如下：

$$x_k=f(x_{k-1},u_k)+w_k$$
$$z_k=h(x_k)+v_k$$
其中，各个参数的物理含义和上面一样，$f(\cdot )$和$h(\cdot )$都是非线形函数。

EKF五大核心公式

运动方程：
$${\hat{X}}_{k+1}^{-}=f(x_{k|k})+f^{\prime }(x_{k|k})(x-x_{k|k})+w_k$$

协方差矩阵：
$$P_{k+1}^{-}=F_K{P}_K{F}_K^T+Q_k$$
kalman增益：
$$K_{k+1}=P_{k+1}^{-}{H}_{k+1}^T(H_{k+1}{P}_{k+1}^{-}{H}_{k+1}^T+R_{k+1}{)}^{-1}$$
状态更新：
$${\hat{X}}_{k+1}={\hat{X}}_{k+1}^{-}+K$$
协方差矩阵更新：
$$P_{k+1}=P_{k+1}^{-}-K_{k+1}{H}_{k+1}{P}_{k+1}^{-}$$

编写程序时就是方程1到方程5的不断循环更新。

EKF的Matlab实现

现在假设运动方程为：
$$X_{k+1}=\frac{1}{2}{X}_k+2.5\frac{X_k}{1+X_k^2}+8cos(1.2k+w_k)$$
观测方程：
$$Z_k=\frac{X_k^2}{20}+v_k$$

现在计算出运动方程对$X$的导数为
$$0.5+2.5\frac{1-X^2}{(1+X^2{)}^2}$$
观测方程对$X$的导数为$\frac{X}{10}$
他们分别作为KEF中的$F$和$H$

Matlab代码如下：

clear;
clc; 
T = 100; 
Q = 10;w=sqrt(Q)*randn(1,T);    % 运动方程中的噪声
R = 1;v=sqrt(R)*randn(1,T);     % 观测方程中的噪声
P = eye(1);                     % 协方差矩阵

x=zeros(1,T);                   % 存储真实的状态
X=zeros(1,T);                   % 存储预测的状态

x(1,1)=0.1;                     % 初始值，运动方程是需要根据上一状态确定下一时刻状态
X(1,1)=x(1,1);

z=zeros(1,T);                   % 存储观测值

for k = 2 : T 
    x(:,k) = 0.5 * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1).^2)) + 8 * cos(1.2*(k-1)) + w(k-1);  
    z(k) = x(:,k).^2 / 20 + v(k); 
    X_ = 0.5*X(:,k-1)+ 2.5*X(:,k-1)/(1+X(:,k-1).^2) + 8 * cos(1.2*(k-1));
    F = 0.5 + 2.5 * (1-X.^2)/((1+X.^2).^2); %运动方程对x求导作为F
    H = X_/10;                              %观测方程对x求导作为H
    P_=F*P*F'+Q;
    K=P_*H'/(H*P_*H'+R);                    %计算kalman增益
    X(k)=X_+K*(z(k)-X_.^2/20);              %更新估计值
    P=P_-K*H*P_;                            %更新协方差矩阵
end

% 误差分析 
Xstd=zeros(1,T); 
for k=1:T 
    Xstd(k)=abs(X(k)-x(k) ); 
end

figure 
hold on;box on; 
plot(x,'-ko','MarkerFace','g');     % k表示线条黑色，o表示数据点是圆圈，markerface表示设置数据点的填充色，g表示数据点是绿色
plot(X,'-ks','MarkerFace','b');     % b表示数据点是蓝色
legend('真实值','Kalman滤波估计值') 
xlabel('时间/s'); 
ylabel('状态值x'); 
figure
plot(Xstd,'-ko');

执行结果如下：

计算出本次sum(Xstd)为144.5709

将x(1,1)设置为1，X(1,1)初始值为0(也就是说初始状态估计不对)时，执行结果如下：

计算本次sum(Xstd)为183.1328

EKF的优缺点

优点：

1. 处理非线性系统：EKF可以处理一类非线性系统，通过在线性化非线性方程来近似处理状态转移和观测模型。这使得EKF可以在许多实际问题中应用，因为很多现实世界中的系统都具有一定的非线性特性。

2. 高效实现：相对于一些更复杂的非线性滤波方法（如粒子滤波器），EKF的计算复杂度较低，计算效率较高，特别是对于中小规模问题。

缺点：

1. 对线性化误差敏感：EKF在线性化非线性方程时引入了误差，特别是对于高度非线性的系统，这种线性化误差可能会显著影响估计结果。在非线性程度较高的情况下，EKF可能会产生较差的估计结果，甚至可能出现发散。

2. 对初始估计敏感：初始状态估计的准确性对EKF的性能影响较大。如果初始估计偏离真实状态较远，EKF可能需要更长的时间来收敛到准确的估计值。

3. 高噪声敏感：EKF假设系统噪声为高斯白噪声，当系统噪声存在非高斯性或噪声较大时，EKF的性能可能会受到影响。

4. 维度灾难：随着状态向量维度的增加，EKF的计算复杂度呈指数增长，这导致在高维问题中，EKF可能变得不可行或效率较低。这也是EKF在大规模或高维问题中应用受限的一个问题。