Kalman Le filtre d'une ligne droite ne converge pas

Question

J'essaie d'écrire un algorithme qui estime et suit une ligne droite: y [k] = b1 * x [k] + b2 [k]. Dans le système physique réel avec lequel je travaille, je peux seulement mesurer y [k], et pour le contrôler, l'entrée est x [k] (j'entre x [k] et j'attends un y [k] spécifique). Le problème est que la relation de y [k] et x [k] n'est pas constante: la pente b1 est constante pour toute itération k, mais pas la constante b2 [k]. Une autre chose que j'ai supposé était que: deltab2 [k] = b2 [k] -b2 [k-1], et c'est constant pour chaque itération.

J'ai essayé d'utiliser le filtre de Kalman, avec le vecteur d'état = (x [k], b2 [k], Delatb2 [k]), et la mesure = y [k]. Cela n'a pas fonctionné - le gain de kalman est devenu pratiquement nul et la matrice de covariance d'erreur n'a pas convergé. J'ai compris que la question de la convergence concerne l'observabilité du système. Cependant j'ai un peu de mal à rendre mon modèle observable. Comment puis-je faire fonctionner mon algorithme?

% note - y[k] is beta here, x[k] is v. 

A=[1 0 -1/b1;0 1 1;0 0 1]; 
H=[b1 1 0]; 

% varb2 = b2[k] variance 
% varb2' = b2[k-1] variance 
% varbeta = measurement noise variance 
% covbbt = b2[k], b2[k-1] covariance - assumed to b2 0 

Qk=varb2*[1/b1^2 -1/b1 -1/b1;-1/b1 1 1; -1/b1 1 1]+covbbt*[0 0 1/b1; 0 0 -1; 1/b1 -1 -2]+varb2t*[0 0 0; 0 0 0; 0 0 1]+varbeta*[1 0 0; 0 0 0; 0 0 0]; 
Rk=varbeta; 
P=Qk; 
x=[5,handles.b(2),0].'; %Assuming the initial drift is 0 

% b1 is assumed to be 200, b2[k=1] assumed to be -400 

%% the algorithm 
v=5; 
while(get(handles.UseK,'Value')) 
    %get covariances 
    x_est=A*x 
    P_est=A*P*A.'+Qk 
    sample_vector = handles.s_in1.startForeground(); 
    I = mean(sample_vector(:,2));% average of the 200 samples 
    Q = mean(sample_vector(:,1));% average of the 200 samples 
    beta=unwrap(atan2(I,Q)); % measurment of beta 
    K=P*H.'*inv(H*P*H.'+Rk) %kalman gain 
    x=x_est+K*(beta-H*x_est) 
    P=P_est-K*H*P_est 
    vo=v; 
    v=x(1); 
    outputSingleScan(handles.s_output1,v); 
end 

Answer 1

(je vais vous en assumerez connaissez le b1)

Dans votre approche, tout cela se résume à la façon dont vous connaissez deltab2. Si vous n'avez pas de très bonne idée, le problème devient assez difficile. Si vous avez une forte supposition pour deltab2, vous pouvez donner cette information en tant qu'avant (état initial) à votre algorithme.

En supposant que vous avez une forte supposition d'abord pour deltab2, vous pouvez essayer quelque chose comme ceci:

% State is [x[k], b2[k], deltab2[k]] 
state = [0 0 deltab2] 
C = [100 0 0;0 100 0;0 0 0.001]; 

% We predict with the dynamics we know of 
A = [1 0 0;0 1 1;0 0 1]; 

% The observation model assumes we know b1 
H = [b1 1 0]; 

% Identity process noise 
Q = [1 0 0;0 1 0;0 0 0.001]; 

% Some observation noise 
R = 1; 

% Assume observations are stacked in vector y 
for i = 1:size(y,1) 
    m = state(end,:)'; 
    P = C(:,:,end); 

    % Predict 
    M = A * M; 
    P = A * P * A' + Q; 


    % Update 
    mu = H * M; 
    nu = y(i) - mu; 

    S = H * P * H' + R; 
    K = P * H'/S; 

    M = M + K * nu; 
    P = P - K * S * K'; 

    state(end+1,:) = M; 
    C(:,:,end+1) = P; 
end 

Il est évident que si vous avez de meilleures suppositions pour le bruit de processus et/ou le bruit d'observation, vous pouvez les utiliser. Dans ce qui précède, nous utilisons la dynamique que nous connaissons: b2 change par deltab2 et deltab2 est constant (avec une estimation initiale forte). Tout le reste est inconnu.

Dans votre approche, vous aviez défini d'autres hypothèses dans la dynamique de A. Je ne sais pas où/comment vous en êtes arrivé, mais si les seules choses connues du système sont que deltab2 est constant et b2 change selon deltab2 , vous ne devriez rien ajouter à A à part ça.

Enfin, j'ai effectué quelques tests avec le bloc de code ci-dessus. Vous obtiendrez de très bonnes estimations pour x [k] en fonction de votre connaissance du deltab2. Si vous n'avez aucune idée de deltab2, vous pouvez obtenir une très bonne prédiction, mais l'estimation x [k] ne correspond pas très bien aux valeurs réelles.

Espérons que cela aide! Si j'ai manqué quelque chose (comme des informations supplémentaires), s'il vous plaît commentez et je peux éditer ma réponse en conséquence!