MATLAB程序演示与编程思路，相对导航，四个小车的形式，使用集中式扩展卡尔曼滤波（fullyCN-EKF）

二维情况下，四个小车各自有绝对定位（GNSS），相互之间部分有相对定位（UWB）时，一个滤波器搞定四个小车的状态滤波。使用EKF作为滤波方法

建模

四辆小车组成编队，每辆车有$x$、$y$两个轴，所以状态量有4*2=8维，观测量为各自的$GNSS$绝对定位+相对定位（1对2+2对1+2对3+3对3+4对3），所以观测量Z的维度为（4+5）*2=18维。
定义系统误差矩阵Q和观测误差协方差矩阵R为：

Q = 0.1*diag(ones(8,1)) %过程噪声协方差
R = 0.1*diag([ones(18,1)]) %观测噪声协方差

定义四个小车的初值为：（1,0）、（2,0）、（3,0）、（4,0），各自的状态方程设置简单一点，统一为：
$${\dot{X}}_{k+1}=f(X_k)+u(k)=X_k+(\begin{array}{c}\cos (0.1k)\\ \cos (0.1k)+1)\end{array})$$
因此四个小车混在一起，可以得到状态量：
$${\mathbf{x}}_k={\left[{\left({\mathbf{x}}_k^1\right)}^{\mathrm{T}},{\left({\mathbf{x}}_k^2\right)}^{\mathrm{T}},\dots ,{\left({\mathbf{x}}_k^4\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$
X对应的状态变换矩阵为8维的主对角矩阵：

F = eye(8);

观测数据生成:

Q_abs = 0.2*diag([1,1]);w_abs=sqrt(Q_abs)*randn(size(Q_abs,1),length(t));Zflight.a = flight.a+w_abs; %生成飞机a的绝对观测量
Q_abs = 0.2*diag([1,1]);w_abs=sqrt(Q_abs)*randn(size(Q_abs,1),length(t));Zflight.b = flight.b+w_abs;
Q_abs = 0.2*diag([1,1]);w_abs=sqrt(Q_abs)*randn(size(Q_abs,1),length(t));Zflight.c = flight.c+w_abs;
Q_abs = 0.2*diag([1,1]);w_abs=sqrt(Q_abs)*randn(size(Q_abs,1),length(t));Zflight.d = flight.d+w_abs;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.ab = flight.a-flight.b+w_coo; %生成a-b的相对观测量
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.ac = flight.a-flight.c+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.ad = flight.a-flight.d+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.ba = flight.b-flight.a+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.bc = flight.b-flight.c+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.bd = flight.b-flight.d+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.ca = flight.c-flight.a+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.cb = flight.c-flight.b+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.cd = flight.c-flight.d+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.da = flight.d-flight.a+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.db = flight.d-flight.b+w_coo;
Q_coo = 0.1*diag([1,1]);w_coo=sqrt(Q_coo)*randn(size(Q_coo,1),length(t));Zflight.dc = flight.d-flight.c+w_coo;

再定义观测方程：

Z_hat =[Xpre(1);Xpre(2);Xpre(3);Xpre(4);Xpre(5);Xpre(6);Xpre(7);Xpre(8);Xpre(1*2-1)-Xpre(2*2-1);Xpre(1*2)-Xpre(2*2); %ZabXpre(2*2-1)-Xpre(1*2-1);Xpre(2*2)-Xpre(1*2); %ZbaXpre(2*2-1)-Xpre(2*3-1);Xpre(2*2)-Xpre(2*3); %ZbcXpre(2*3-1)-Xpre(2*2-1);Xpre(2*3)-Xpre(2*2);%ZcbXpre(2*4-1)-Xpre(2*3-1);Xpre(2*4)-Xpre(2*3)%Zdc];

上述代码第一行代表四个小车的绝对观测量（每个小车2个维度，所以共8维）。
第二行表示第一个的坐标减去第二个的坐标（2→1的观测量），第三行至第六行以此类推。
由Z可以得到观测矩阵：

H = [1,0,0,0,0,0,0,0;0,1,0,0,0,0,0,0;0,0,1,0,0,0,0,0;0,0,0,1,0,0,0,0;0,0,0,0,1,0,0,0;0,0,0,0,0,1,0,0;0,0,0,0,0,0,1,0;0,0,0,0,0,0,0,1;1,0,-1,0,0,0,0,0;0,1,0,-1,0,0,0,0;-1,0,1,0,0,0,0,0;0,-1,0,1,0,0,0,0;0,0,1,0,-1,0,0,0;0,0,0,1,0,-1,0,0;0,0,-1,0,1,0,0,0;0,0,0,-1,0,1,0,0;0,0,0,0,-1,0,1,0;0,0,0,0,0,-1,0,1];

滤波

PP=F*P*F'+Q;
Kk=PP*H'/(H*PP*H'+R);
flight_ekf.fully(:,k)=Xpre+Kk*(flightZ.fully(:,k)-Z_hat);
P=PP-Kk*H*PP;
flightP_num.fully(k,:,:) = P;

运行结果——演示

运行结果——绘图

a机的x轴与y轴位移：

a机的x轴与y轴误差曲线图：

a机x轴和y轴的累积误差概率曲线图：

b机x轴和y轴的累积误差概率曲线图：

c机x轴和y轴的累积误差概率曲线图：

d机x轴和y轴的累积误差概率曲线图：

程序个性化较强，尚未整理成完善的、通用化的例程，如有需要定制或咨询，可通过下方卡片联系作者交流