El principio del algoritmo de filtro de Kalman y su aplicación en un problema de predicción (MATLAB)

principio

El núcleo del algoritmo de filtrado de Kalman son las cinco fórmulas, que incluyen el valor medido, el valor a priori y un valor posterior. El valor anterior es en realidad un valor aproximado calculado de acuerdo con el modelo del sistema (el modelado es inexacto) y, al mismo tiempo, podemos usar el equipo de prueba para medir un valor de medición. El problema ahora es cómo fusionar estos dos valores Para calcular un valor posterior en este momento, este valor posterior debe cumplir con el principio de varianza del error mínimo (en este momento, es el más cercano al valor real). El algoritmo de filtrado de Kalman es un problema de optimización matemática que se ha deducido rigurosamente. No se deducirá aquí. Se dan las cinco fórmulas del algoritmo final: X ^ 1, k - \ hat {X} ^ -_ {1, k } en la

fórmula${\hat{X}}_{1,k}^{-}$Es la posición anterior, ${\hat{X}}_{1,k}$Es el valor posterior (el resultado del filtrado de Kalman en el k-ésimo paso). En el filtrado de Kalman, es necesario inicializar varios datos, a saber, X en el momento 0 $\hat{X}$ YP k P_ken el momento 1${PAG}_k$, Luego, el valor anterior se puede calcular de acuerdo con la primera fórmula en el momento 1, y el nuevo P k - P ^ -_ k se puede calcular de acuerdo con la segunda fórmula${PAG}_k^{-}$(A priori), entonces K k K_k se calcula de acuerdo con la tercera fórmula$K_k$(El lado derecho de esta fórmula es ${PAG}_k^{-}$Incorrecto), la cuarta fórmula realiza la fusión de datos del valor anterior y el valor medido, y la última fórmula actualiza ${PAG}_k$。

MATLAB implementa un ejemplo de filtro de Kalman

Aquí asumimos que un objeto se mueve en un espacio unidimensional, con dos variables de estado, velocidad y posición. Inicialmente en la posición 0, la velocidad es 1 y el período de muestreo del sistema es 1 s. El observador de satélite existente observa su posición y velocidad , pero ambos existen. Ruido, ya hemos modelado este modelo de segundo orden (la posición en el tiempo k es la posición en el tiempo k-1 + velocidad * 1s + ruido, y la velocidad en el tiempo k es la velocidad en el tiempo k-1: + ruido), y ahora usamos el algoritmo de filtro de Kalman para filtrar.

el código se muestra a continuación:

%------------------Kalman filter----------------%
%This is an application of Kalman filter algorithm in a simple problem which needs
%us to estimate the displacement and speed. The initial condition is [0;1], which 
%means the x0 = 0, v0 = 1. Besides, the interval time is 1s. p(w) and p(v) are the noise
%which obey normal distribution and the covariance matrices are Q and R respectively.

H = eye(2); A = [1 1;0 1];
Pk = eye(2); Q = 0.1*eye(2);R = eye(2);
X_k = [0;1]; X_hat = [0;1];

X_kdata = [0;1]; X_hatdata = [0;1];
X_hat_data = []; Z_kdata = [];
iter = 20;
for i = 1:iter
    pw = mvnrnd([0 0],[0.1 0;0 0.1],1)';
    pv = mvnrnd([0 0],[1 0;0 1],1)';
    X_k = A*X_k + pw; X_kdata = [X_kdata,X_k];
    Z_k = H*X_k + pv; Z_kdata = [Z_kdata,Z_k];
    
    Pk_ = A*Pk*A'+Q;
    Kk = Pk_*H'*inv(H*Pk_*H'+R);
    X_hat_ = A*X_hat; 
    X_hat_data = [X_hat_data,X_hat_];
    X_hat = X_hat_ + Kk*(Z_k - H*X_hat_);
    X_hatdata = [X_hatdata,X_hat];
    Pk = (eye(2) - Kk*H)*Pk_;
end

figure(1);
plot(0:iter,X_kdata(1,:),0:iter,X_hatdata(1,:)...
    ,1:iter,X_hat_data(1,:),1:iter,Z_kdata(1,:)...
    ,0:iter,X_kdata(2,:),0:iter,X_hatdata(2,:)...
    ,1:iter,X_hat_data(2,:),1:iter,Z_kdata(2,:));
legend("实际位置","后验位置","先验位置","测量位置",...
    "实际速度","后验速度","先验速度","测量速度"...
    ,"Location","best");
grid on; title("Kalman filter");
xlabel("iter"); ylabel("x or v");

• resultado
  
• Zoom parcial
  

Se puede ver que el filtro de Kalman es en realidad un proceso de fusión de datos. La medición está sesgada al cálculo cuando el cálculo es inexacto, y el cálculo está sesgado a la medición cuando la medición es inexacta. Coeficiente de fusión $K_k$Se obtiene optimizando la varianza del error.