EKF简介
首先kalman滤波器的作用是在包含不确定性的动态系统中做最优估计,滤波也就是估计的意思。但是kalman滤波器只能对线性系统进行估计,它要求状态转移方程和观测方程都是线性的,具体就是,满足如下的状态转移方程:
x k = F k ∗ x k − 1 + B k ∗ u k + w k x_k = F_k * x_{k-1} + B_k * u_k + w_k xk=Fk∗xk−1+Bk∗uk+wk
其中,
- x k x_k xk 是当前时间步的状态向量(待估计的状态)。
- F k F_k Fk 是状态转移矩阵,表示状态如何从上一时刻转移到当前时刻。
- x k − 1 x_{k-1} xk−1 是上一时刻的状态向量。
- B k B_k Bk 是控制输入矩阵,用于表示外部控制对系统状态的影响(在某些情况下可能不存在)。
- u k u_k uk 是当前时间步的控制输入向量。
- w k w_k wk 是过程噪声,假设为高斯白噪声,表示状态转移过程中的不确定性或随机扰动。
满足如下的观测方程:
z k = H k ∗ x k + v k z_k = H_k * x_k + v_k zk=Hk∗xk+vk
其中,
- z k z_k zk 是当前时间步的观测向量(通过传感器等获取的实际测量值)。
- H k H_k Hk 是观测矩阵,用于将状态映射到观测空间。
- x k x_k xk 是当前时间步的状态向量。
- v k v_k vk 是观测噪声,假设为高斯白噪声,表示观测过程中的不确定性或随机误差。
而EKF满足的状态转移方程和观测方程如下:
x k = f ( x k − 1 , u k ) + w k x_k = f(x_{k-1}, u_k) + w_k xk=f(xk−1,uk)+wk
z k = h ( x k ) + v k z_k = h(x_k) + v_k zk=h(xk)+vk
其中,各个参数的物理含义和上面一样, f ( ⋅ ) f(·) f(⋅)和 h ( ⋅ ) h(·) h(⋅)都是非线形函数。
EKF五大核心公式
运动方程:
X ^ k + 1 − = f ( x k ∣ k ) + f ′ ( x k ∣ k ) ( x − x k ∣ k ) + w k \hat{X}^-_{k+1}=f(x_{k|k})+f'(x_{k|k})(x-x_{k|k})+w_k X^k+1−=f(xk∣k)+f′(xk∣k)(x−xk∣k)+wk
协方差矩阵:
P k + 1 − = F K P K F K T + Q k P_{k+1}^-=F_KP_KF_K^T+Q_k Pk+1−=FKPKFKT+Qk
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 K_{k+1} =P^-_{k+1}H^T_{k+1}(H_{k+1}P^-_{k+1}H^T_{k+1}+R_{k+1})^{-1} Kk+1=Pk+1−Hk+1T(Hk+1Pk+1−Hk+1T+Rk+1)−1
状态更新:
X ^ k + 1 = X ^ k + 1 − + K \hat{X}_{k+1}=\hat{X}_{k+1}^- + K X^k+1=X^k+1−+K
协方差矩阵更新:
P k + 1 = P k + 1 − − K k + 1 H k + 1 P k + 1 − P_{k+1}=P^-_{k+1}-K_{k+1}H_{k+1}P^-_{k+1} Pk+1=Pk+1−−Kk+1Hk+1Pk+1−
编写程序时就是方程1到方程5的不断循环更新。
EKF的Matlab实现
现在假设运动方程为:
X k + 1 = 1 2 X k + 2.5 X k 1 + X k 2 + 8 c o s ( 1.2 k + w k ) X_{k+1} = \frac{1}{2}X_k+2.5\frac{X_k}{1+X_k^2}+8cos(1.2k+w_k) Xk+1=21Xk+2.51+Xk2Xk+8cos(1.2k+wk)
观测方程:
Z k = X k 2 20 + v k Z_k = \frac{X_k^2}{20}+v_k Zk=20Xk2+vk
现在计算出运动方程对 X X X的导数为
0.5 + 2.5 1 − X 2 ( 1 + X 2 ) 2 0.5+2.5\frac{1-X^2}{(1+X^2)^2} 0.5+2.5(1+X2)21−X2
观测方程对 X X X的导数为 X 10 \frac{X}{10} 10X
他们分别作为KEF中的 F F F和 H H 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的优缺点
优点:
-
处理非线性系统:EKF可以处理一类非线性系统,通过在线性化非线性方程来近似处理状态转移和观测模型。这使得EKF可以在许多实际问题中应用,因为很多现实世界中的系统都具有一定的非线性特性。
-
高效实现:相对于一些更复杂的非线性滤波方法(如粒子滤波器),EKF的计算复杂度较低,计算效率较高,特别是对于中小规模问题。
缺点:
-
对线性化误差敏感:EKF在线性化非线性方程时引入了误差,特别是对于高度非线性的系统,这种线性化误差可能会显著影响估计结果。在非线性程度较高的情况下,EKF可能会产生较差的估计结果,甚至可能出现发散。
-
对初始估计敏感:初始状态估计的准确性对EKF的性能影响较大。如果初始估计偏离真实状态较远,EKF可能需要更长的时间来收敛到准确的估计值。
-
高噪声敏感:EKF假设系统噪声为高斯白噪声,当系统噪声存在非高斯性或噪声较大时,EKF的性能可能会受到影响。
-
维度灾难:随着状态向量维度的增加,EKF的计算复杂度呈指数增长,这导致在高维问题中,EKF可能变得不可行或效率较低。这也是EKF在大规模或高维问题中应用受限的一个问题。