基于卡尔曼滤波的三种经典室内定位算法Fang、Chan、Taylor

基于卡尔曼滤波的三种经典室内定位算法：Fang、Chan、Taylor，MATLAB仿真。

 function main 
clc;
clear;  
T=1;%扫描周期
N=120/T;%采样次数  
X=zeros（4N）;%目标真实位置、速度 
X（:1）=[302402];%目标初始位置（3040）速度（220）
Z=zeros（2N）;%基站对位置的观测 
a=exprnd（214）;%指数分布的过程噪声
Q=diag（a）;%过程噪声
b=exprnd（312）;
R=60*diag（b）; %观测噪声均值 指数分布
A=[1T00;
   0100;
   001T;
   0001]; %状态转移矩阵 
G=[1000;
   0010];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
for t=2:N      
    X（:t）=A*X（:t-1）+sqrtm（Q）*randn（41）;%目标真实轨迹     
end
MX=X（1:）;
MY=X（3:）;
for t=1:N
    yuce=zz（MX（1t）MY（1t））;
    Z（:t）=[yuce（1）;yuce（2）]+sqrtm（R）*randn（21）;%对目标的观测
end
% Kalman滤波 
Xkf=zeros（4N）; %滤波后的最优估计 
Xkf（:1）=X（:1）;%初始化 
P0=eye（4）;% 误差协方差阵初始化 
for i=2:N      
    %预测方程
    Xn=A*Xkf（:i-1）;%状态预测     
    P1=A*P0*A‘+Q;%预测误差协方差  
    %信息方程
    K=P1*G‘/（G*P1*G‘+R）;%Kalman增益
    %估计方程
    Xkf（:i）=Xn+K*（Z（:i）-G*Xn）;%状态更新     
    P0=（eye（4）-K*G）*P1;%滤波误差协方差更新 
end
% 误差分析 
for i=1:N      
    Observation（i）=RMS（X（:i）Z（:i））;%滤波前的误差     
    KalmanFilter（i）=RMS（X（:i）Xkf（:i））;%滤波后的误差 
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% 画图 
figure 
hold on;box on;  
plot（X（1:）X（3:）‘-k‘）;%真实轨迹 
plot（Z（1:）Z（2:）‘*‘）;%观测轨迹  
plot（Xkf（1:）Xkf（3:）‘-r‘）;%Kalman滤波轨迹 
legend（‘真实轨迹‘‘观测轨迹‘‘滤波后轨迹‘） 
xlabel（‘横坐标 X/m‘）; ylabel（‘纵坐标 Y/m‘）; 
figure 
hold on;box on;  
plot（Observation‘-ko‘‘MarkerFace‘‘g‘） 
plot（KalmanFilter‘-ks‘‘MarkerFace‘‘r‘） 
legend（‘滤波前误差‘‘滤波后误差‘）
xlabel（‘观测时间/s‘）; ylabel（‘误差值‘）; 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1690  2017-11-19 10:34  卡尔曼定位程序\chan\main.m

     文件        178  2017-11-06 11:06  卡尔曼定位程序\chan\RMS.m

     文件       2522  2017-11-19 11:04  卡尔曼定位程序\chan\zz.m

     文件       1109  2017-11-19 11:30  卡尔曼定位程序\fang\Fang.m

     文件       1723  2017-11-17 19:23  卡尔曼定位程序\fang\main.m

     文件        178  2017-11-06 11:06  卡尔曼定位程序\fang\RMS.m

     文件       1727  2017-11-17 19:42  卡尔曼定位程序\SI\main.m

     文件        178  2017-11-06 11:06  卡尔曼定位程序\SI\RMS.m

     文件       1107  2017-11-19 11:50  卡尔曼定位程序\SI\SI.m

     文件       1730  2017-11-17 19:57  卡尔曼定位程序\Taylor\main.m

     文件        178  2017-11-06 11:06  卡尔曼定位程序\Taylor\RMS.m

     文件       1342  2017-11-19 12:07  卡尔曼定位程序\Taylor\Taylor.m

     目录          0  2018-01-02 21:15  卡尔曼定位程序\chan

     目录          0  2018-01-02 21:15  卡尔曼定位程序\fang

     目录          0  2018-01-02 21:15  卡尔曼定位程序\SI

     目录          0  2018-01-02 21:15  卡尔曼定位程序\Taylor

     目录          0  2018-01-02 21:15  卡尔曼定位程序

----------- ---------  ---------- -----  ----

                13662                    17