Sage-Husa自适应滤波

Sage-Husa自适应卡尔曼滤波

Sage-Husa 自适应滤波是在利用观测数据递推滤波时，实时估计和修正系统噪声和观测噪声的统计特性，从而降低系统模型误差，提高滤波精度。

首先列出系统方程如下：

$x_k=A{x}_{k-1}+B{u}_k+C{w}_k$

$y_k=H{x}_k+v_k$

其中， $x_k,y_k,u_k$ 分别表示状态量，输出量，输入量。 $w_k,v_k$ 为噪声

$E\left[w_k\right]=q_k$

$E\left[w_k{w}_j^T\right]=Q_k{\delta }_{kj}$

$E\left[v_k\right]=r_k$

$E\left[v_k{v}_j^T\right]=R_k{\delta }_{kj}$

$E\left[w_k{v}_j^T\right]=0$

$Q_k,R_k,q_k,r_k$ 将在后续进行更新，这也是区别于卡尔曼滤波的最主要的地方。

那么，Sage-Husa滤波器的公式如下。

1）计算一步预测方程

${\hat{x}}_{k,k-1}=A{\hat{x}}_{k-1}+B{u}_{k-1}+C{\hat{q}}_{k-1}$

这里的 ${\hat{x}}_{k,k-1},{\hat{q}}_{k-1}$ 的上标表示为估计值。卡尔曼滤波器的对应公式为，即 $q$为0。

${\hat{x}}_{k,k-1}=A{\hat{x}}_{k-1}+B{u}_{k-1}$

2）一步预测均方误差方程

$P_{k,k-1}=A{P}_{k-1}{A}^T+C{\hat{Q}}_{k-1}{C}^T$

卡尔曼滤波器的对应公式为，也就是 $Q$ 为常数

$P_{k,k-1}=A{P}_{k-1}{A}^T+CQ{C}^T$

3）更新滤波增益方程

$K_k=P_{k,k-1}{H}^T{\left[H{P}_{k,k-1}{H}^T+{\hat{R}}_{k-1}\right]}^{-1}$

卡尔曼滤波器的对应公式为，也就是 $R$ 常数

$K_k=P_{k,k-1}{H}^T{\left[H{P}_{k,k-1}{H}^T+R\right]}^{-1}$

4）计算残差

${\epsilon }_k=y_k-H{\hat{x}}_{k,k-1}-{\hat{r}}_{k-1}$

卡尔曼滤波器的对应公式为，也就是 $r$ 为0

${\epsilon }_k=y_k-H{\hat{x}}_{k,k-1}$

5）k时刻的状态估计

${\hat{x}}_k={\hat{x}}_{k,k-1}+K_k{\epsilon }_k$

卡尔曼滤波器的对应公式是一样的。

6）k时刻的均方误差方程

$P_k=\left[I-K_{k}H\right]P_{k,k-1}$

卡尔曼滤波器的对应公式是一样的。

接下来开始更新 ${\hat{q}}_k,{\hat{Q}}_k,{\hat{r}}_k,{\hat{R}}_k$，以下部分卡尔曼滤波器是没有的。

7）计算加权系数

$d_k=\frac{1-b}{1-b^{k+1}},0<b<1$

从中以及下面的公式可知，随着时间的推移， $1-d_k$ 趋近于 $b$。

还有一种加权系数形式为$d_k=\frac{1}{k}$

从中以及下面的公式可知，随着时间的推移， $1-d_k$ 趋近于1。如果令 $d_k=0$ ，则还原为普通卡尔曼滤波器了，也就是说采用这种形式的 $d_k$，随着时间的推移，还原为普通卡尔曼滤波器了。

8）更新 ${\hat{q}}_k,{\hat{Q}}_k,{\hat{r}}_k,{\hat{R}}_k$

${\hat{q}}_k=\left(1-d_{k-1}\right){\hat{q}}_{k-1}+d_{k-1}\left({\hat{x}}_k-A{\hat{x}}_{k-1}\right)$

${\hat{Q}}_k=\left(1-d_{k-1}\right){\hat{Q}}_{k-1}+d_{k-1}\left(K_k{\epsilon }_k{\epsilon }_k^T{K}_k^T+P_k-A{P}_{k-1}{A}^T\right)$

${\hat{r}}_k=\left(1-d_{k-1}\right){\hat{r}}_{k-1}+d_{k-1}\left(y_k-H{\hat{x}}_{k,k-1}\right)$

${\hat{R}}_k=\left(1-d_{k-1}\right){\hat{R}}_{k-1}+d_{k-1}\left({\epsilon }_k{\epsilon }_k^T-H{P}_{k,k-1}{H}^T\right)$

如果系统维数较高,而 Sage-Husa自适应滤波算法中增加对系统噪声统计特性的计算，计算量将增加，实时性将难以保证。此外，对于阶次较高的系统，算法中R和Q的在线估计有时会由于计算发散失去半正定性和正定性而出现滤波发散现象，此时滤波算法的稳定性和收敛性不能完全保证。

代码如下，供大家参考。

clc
clear
a=randn(1,5000)*sqrt(0.1);
y=zeros(1,5000);
y(1)=a(1);
y(2)=a(2)-0.575*a(1);
for k=3:5000
y(k)=0.0673*y(k-1)+0.1553*y(k-2)+a(k)-0.575*a(k-1);
end
y = y+sqrt(0.05)*randn(1,length(y));
figure(1)
subplot(2,1,1)
plot(a),title('a');
subplot(2,1,2)
plot(y),title('y');
b=0;
% inlitial Kalman Filter
F = [0.0673 0.1553; 1 0];
G = [1 -0.575 ; 0 0];
H = [1 0];
s=1*eye(2);
Q0=diag([0.1,0.1]);
R0 = 0.5;
P0=eye(2)*10000;
X0=[0;0];
[X,e,P]=Sage_HusaKF(F,G,H,Q0,R0,X0,y,P0,b,s);
% [X,e,P]=StdKF2(F,G,H,Q0,R0,X0,y,P0);
% plot kalman
figure(2)
t=1:length(X);
subplot(2,1,1)
plot(t,X(1,:));%yk
subplot(2,1,2)
plot(t,X(2,:));%yk-1
figure(3)
plot(t,y);
hold on
plot(t,y,'r',t,X(1,:),'b');
hold off

%% 
function [X,e,P]=Sage_HusaKF(F,G,H,Q0,R0,X0,Z,P0,b,s);
% X   是指得到的k状态下的最优的估算值
% e   残差epsilon(k)
% P   是指更新的k状态下X对应的协方差

% F   系统参数，对于多模型系统，它们为矩阵
% G   系统参数，对于多模型系统，它们为矩阵
% H   表示协方差的转换系数
% Q0  表示系统过程的协方差初值
% R0  表示某种不确定性(传感器噪声)的协方差初值
% X0  是最优的估算值初值  
% Z   表示k 时刻状态下的观测值。
% P0  是X对应的协方差初值
% b   sage-Husa更新时用到，未用到默认
% s   2*2的单位矩阵，初值

% Sage-Husa adeptive KF

N=length(Z);
M=length(X0);
X=zeros(M,N);
X(:,1)=X0;
s=1*eye(2);
P=P0;
q0 = zeros(M,1);
r0 = 0;
q = q0;
r = r0;
Q = Q0;
R = R0;
for k=2:N
    X_est=F*X(:,k-1)+q;                  %计算一步预测估计：X(k/k-1)
    P_pre=F*P*F'+G*Q*G';               %一步预测估计的均方误差P(k/k-1)
    e(:,k)=Z(:,k)-H*X_est-r;           %计算残差epsilon(k)
    K=P_pre*H'*inv((H*P_pre*H')+R);    %k时刻的增益阵
    X(:,k)=X_est+K*e(:,k);           %k时刻的状态估计X(k)
    P = (eye(M)-K*H)*P_pre;%*(eye(M)-K*H)'+K*R*K';  %均方误差矩阵P(k)
    
% %     sage-Husa更新Q，R，q,r
%     d = (1-b)/(1-b^(k));
%     r = (1-d)*r +d*(Z(:,k)-H*X_est);
%     q = (1-d)*q +d*(X(:,k)-F*X(:,k-1));
%     R = (1-d)*R +d*(Z(:,k)*Z(:,k)'-H*P*H');
%     Q = (1-d)*Q +d*(K*e(:,k)*e(:,k)'*K'+P-F*P*F');
%    
      r = 1/k*((k-1)*r +Z(:,k)-H*X_est);
    
      q = 1/k*((k-1)*q+X(:,k)-F*X(:,k-1));
    
      R = 1/k*((k-1)*R+Z(:,k)*Z(:,k)'-H*P*H');
     
      Q = 1/k*((k-1)*Q+K*e(:,k)*e(:,k)'*K'+P-F*P*F');
%     
end
end

基于极大似然准则的自适应滤波

基于极大似然准则的自适应滤波，通过系统状态方差阵和量测噪声方差阵 实时估计系统噪声统计特性的变化，以保证滤波器更好地适应这种变化。极大似然估计从系统观测值出现概率最大的角度估计，其特点是不仅考虑新息的变化，而且考虑新息协方差矩阵的变化。
${\hat{R}}_k=C_{vk}-H{P}_{k,k-1}{H}^T$
${\hat{Q}}_k=\frac{1}{N}{\sum }_{i=k-N+1}^k\Delta x_i\Delta x_i^T+P_k-A^T{P}_{k-1}A$
$\Delta x_k={\hat{x}}_k-{\hat{x}}_{k,k-1}=K_k{v}_k$
$C_{vk}=\frac{1}{N}{\sum }_{i=k-N+1}^k{v}_i{v}_i^T$
式中​ $v_k=y_k-{\hat{y}}_{k,k-1}$， $N$ 为平滑窗口宽度。