Matlab Kalman Filter pour 3D Position

Question

J'essaie de trouver la position 3D en utilisant le filtre de Kalman .Kindly ce serait si gentil si quelqu'un s'il vous plaît aidez-moi comment je peux modifier mon code 1D en 3D. Je suis confus au sujet de ce filtre.

i = 100; % time points 
s=[i, 1]; % size 
r=22; % actual value 
Q=4e-5; % variance 
R=0.23; % measurement variance 
m = r + sqrt(R)*randn(s); % measured value 
v_est=zeros(s); % estimated variace :taken after measurment 
x_hat_minus=zeros(s); %estimation before measurment at n-1 step 
v_minus_est=zeros(s); % variance estimated before measurments 
K=zeros(s); % Kalman factor 
x_hat(1) = 21.5; % estimation starts at this value  
v_est(1) =1; % error in variance 
for n = 2:i  
% step:1 time update 
x_hat_minus(n) = x_hat(n-1); %estimating the current value based on previous estimations  
v_minus_est(n) = v_est(n-1)+Q; %variance prediction 
% step 2: Measurement Update 
K(n) = v_minus_est(n)/(v_minus_est(n)+R); %calculating kalman factor 
x_hat(n) = x_hat_minus(n)+K(n)*(m(n)-x_hat_minus(n)); %estimation  
v_est(n) = (1-K(n))*v_minus_est(n); %variance achieved in last estimation 
end 

Answer 1

Je n'ai pas lu votre code complètement (ne l'ai pas utilisé Matlab pour un an), mais il semble que vous avez mis en œuvre que d'un premier ordre filtre de Kalman (?), Que je crois les noix sur comme un filtre IIR simple une fois le gain réglé.

... Anyways

Pour répondre à votre question: mettre en œuvre simplement 3 filtres de Kalman, un pour chaque axe X , Y et Z

Comme ils sont orthogonales ils sont indépendants les uns des autres et vivent dans leur propre petit univers.

en supposant un ordre unique, vous pouvez construire une matrice d'état 3x3 si vous voulez, mais il est plus transparent pour aller avec 3 1x1 de (ou mieux encore 3 x second ordre 2x2 de, si vous avez pas déjà) .followed