3d 三维位置的Matlab卡尔曼滤波

我正在尝试使用卡尔曼滤波器寻找三维位置。如果有人能帮助我如何将我的一维代码修改为三维，那将是一件非常好的事情。我对这个滤波器感到困惑

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

---

我还没有完全阅读您的代码（大约一年没有使用matlab），但您似乎只实现了一阶Kalman滤波器（？），我相信，一旦增益稳定下来，它就是一个简单的IIR滤波器

无论如何

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

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

假设为单阶，如果您愿意，您可以构建一个3x3状态矩阵，但是使用3个1x1（或者更好的是，如果您还没有创建3个二阶2x2）更透明