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

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

文章目录

建模

四辆小车组成编队,每辆车有 x x x、 y y y两个轴,所以状态量有4*2=8维,观测量为各自的 G N S S GNSS GNSS绝对定位+相对定位(1对2+2对1+2对3+3对3+4对3),所以观测量Z的维度为(4+5)*2=18维。

定义系统误差矩阵Q和观测误差协方差矩阵R为:

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

定义四个小车的初值为:(1,0)、(2,0)、(3,0)、(4,0),各自的状态方程设置简单一点,统一为:
X ˙ k + 1 = f ( X k ) + u ( k ) = X k + ( cos ⁡ ( 0.1 k ) cos ⁡ ( 0.1 k ) + 1 ) ) \dot{X}{k+1}=f(X{k})+u(k)=X_{k}+\left.\left(\begin{matrix}\cos(0.1k)\\\cos(0.1k)+1)\end{matrix}\right.\right) X˙k+1=f(Xk)+u(k)=Xk+(cos(0.1k)cos(0.1k)+1))

因此四个小车混在一起,可以得到状态量:
x k = [ ( x k 1 ) T , ( x k 2 ) T , ... , ( x k 4 ) T ] T \mathbf{x}{k}=\left[\left(\mathbf{x}{k}^{1}\right)^{\mathrm{T}},\left(\mathbf{x}{k}^{2}\right)^{\mathrm{T}},\ldots,\left(\mathbf{x}{k}^{4}\right)^{\mathrm{T}}\right]^{\mathrm{T}} xk=[(xk1)T,(xk2)T,...,(xk4)T]T

X对应的状态变换矩阵为8维的主对角矩阵:

matlab 复制代码
F = eye(8);

观测数据生成:

matlab 复制代码
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;

再定义观测方程:

matlab 复制代码
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); %Zab
    Xpre(2*2-1)-Xpre(1*2-1);Xpre(2*2)-Xpre(1*2); %Zba
    Xpre(2*2-1)-Xpre(2*3-1);Xpre(2*2)-Xpre(2*3); %Zbc
    Xpre(2*3-1)-Xpre(2*2-1);Xpre(2*3)-Xpre(2*2);%Zcb
    Xpre(2*4-1)-Xpre(2*3-1);Xpre(2*4)-Xpre(2*3)%Zdc
    ];

上述代码第一行代表四个小车的绝对观测量(每个小车2个维度,所以共8维)。

第二行表示第一个的坐标减去第二个的坐标(2→1的观测量),第三行至第六行以此类推。

由Z可以得到观测矩阵:

matlab 复制代码
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];

滤波

matlab 复制代码
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;

运行结果------演示

CNconcentrate_Ver1竖屏视频

运行结果------绘图

a机的x轴与y轴位移:

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

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

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

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

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

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

相关推荐
机器学习之心3 小时前
多目标鲸鱼优化算法(NSWOA),含46种测试函数和9个评价指标,MATLAB实现
算法·matlab·多目标鲸鱼优化算法·46种测试函数·9个评价指标
编程武士4 小时前
从50ms到30ms:YOLOv10部署中图像预处理的性能优化实践
人工智能·python·yolo·性能优化
max5006004 小时前
基于Meta Llama的二语习得学习者行为预测计算模型
人工智能·算法·机器学习·分类·数据挖掘·llama
月疯5 小时前
OPENCV摄像头读取视频
人工智能·opencv·音视频
极客天成ScaleFlash5 小时前
极客天成让统一存储从云原生‘进化’到 AI 原生: 不是版本升级,而是基因重组
人工智能·云原生
王哥儿聊AI5 小时前
Lynx:新一代个性化视频生成模型,单图即可生成视频,重新定义身份一致性与视觉质量
人工智能·算法·安全·机器学习·音视频·软件工程
_pinnacle_6 小时前
打开神经网络的黑箱(三) 卷积神经网络(CNN)的模型逻辑
人工智能·神经网络·cnn·黑箱·卷积网络
Ada's6 小时前
深度学习在自动驾驶上应用(二)
人工智能·深度学习·自动驾驶
张较瘦_6 小时前
[论文阅读] 人工智能 + 软件工程 | 从“人工扒日志”到“AI自动诊断”:LogCoT框架的3大核心创新
论文阅读·人工智能·软件工程
lisw056 小时前
连接蓝牙时“无媒体信号”怎么办?
人工智能·机器学习·微服务