Question

我正在尝试使用卡尔曼滤波器找到3D位置。如果有人请帮助我如何将我的1D代码修改为3D，我会很高兴。我对此过滤器感到困惑。

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

我还没有完全阅读你的代码（一年左右没有使用过matlab），但是看起来你只实现了一阶卡尔曼滤波器（？），我相信这只是一个简单的IIR在收益稳定后过滤。

无论如何 ......

回答你的问题：只需实现3个卡尔曼滤波器，每个轴一个 X ， Y 和 Z

因为它们是正交的，所以它们彼此独立并且生活在他们自己的小宇宙中。

假设单个订单，你可以根据需要构建一个3x3状态矩阵，但是使用3个1x1（或者更好的3个二阶2x2，如果你还没有）则更加透明。关注

用于3D位置的Matlab卡尔曼滤波器

1 个答案: