{"id":28378168,"url":"https://github.com/tsinglinrain/ekf_example","last_synced_at":"2025-06-26T21:31:19.229Z","repository":{"id":214566991,"uuid":"736835629","full_name":"tsinglinrain/EKF_example","owner":"tsinglinrain","description":"Extended Kalman Filter(EKF) example of three drones locating a cruise ship","archived":false,"fork":false,"pushed_at":"2024-01-02T11:03:40.000Z","size":447,"stargazers_count":0,"open_issues_count":0,"forks_count":0,"subscribers_count":1,"default_branch":"main","last_synced_at":"2025-06-06T02:21:25.811Z","etag":null,"topics":["ekf","example","matlab"],"latest_commit_sha":null,"homepage":"","language":"TeX","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"mit","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/tsinglinrain.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null}},"created_at":"2023-12-29T02:49:32.000Z","updated_at":"2023-12-29T06:29:06.000Z","dependencies_parsed_at":"2024-01-02T12:24:08.541Z","dependency_job_id":null,"html_url":"https://github.com/tsinglinrain/EKF_example","commit_stats":null,"previous_names":["tsinglinrain/ekf_example"],"tags_count":0,"template":false,"template_full_name":null,"purl":"pkg:github/tsinglinrain/EKF_example","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/tsinglinrain%2FEKF_example","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/tsinglinrain%2FEKF_example/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/tsinglinrain%2FEKF_example/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/tsinglinrain%2FEKF_example/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/tsinglinrain","download_url":"https://codeload.github.com/tsinglinrain/EKF_example/tar.gz/refs/heads/main","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/tsinglinrain%2FEKF_example/sbom","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":262145143,"owners_count":23265875,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["ekf","example","matlab"],"created_at":"2025-05-30T02:05:51.160Z","updated_at":"2025-06-26T21:31:19.220Z","avatar_url":"https://github.com/tsinglinrain.png","language":"TeX","funding_links":[],"categories":[],"sub_categories":[],"readme":"# README_zh\n\n## 一阶扩展卡尔曼滤波\n\n### 一阶扩展卡尔曼滤波原理\n\n标准卡尔曼滤波方法是建立在线性系统和高斯分布两个假设条件之上的,然\n而绝大多数实际系统都存在一定程度的非线性,使得原本符合高斯分布的随机变\n量经非线性变换后不再满足高斯特性,导致卡尔曼滤波无法直接应用.\n\n为解决非线性系统的滤波问题,将非线性系统进行线性化是最直接的做法,\n从而将非线性滤波问题转化为线性滤波问题.线性化是将非线性函数进行 Taylor\n展开或者以多项式的方式进行拟合,然后忽略高阶小项得到偏差线性化\n函数的过程,根据所取阶数不同可以获得不同的近似精度,一般以多阶线性化获得较高近似精度的,相应的计算量也会比较大.\n\nEKF是一种比较简单的将非\n线性方程进行Taylor一阶展开并忽略高阶项的扩展卡尔曼滤波方法,由于其该\n方法只是一阶近似,因此当系统非线性较强时会带来较大的截断误差.但由于\nEKF方法的小计算复杂度带来的简便性,在工程中,对于一些非线性并不强的系\n统仍被广泛使用.\n\n### 一阶扩展卡尔曼滤波方法\n\n对于以下非线性系统:\n\n$$\n    x_k = f(x_{k-1}) + w_{k-1}\\\\\n    z_k = h(x_k) + v_k\n$$\n\n式中, $f(\\cdot)$ 和 $h(\\cdot)$ 为非线性方程; $w_{k-1}$ 和 $v_k$ 为互不相关的高斯高斯白噪声,\n其噪声统计特性为:\n\n$$\n\\begin{aligned}\n    E[w_k] = 0, ~Cov[w_k,w_j] = Q_k\\delta_{k,j}\\\\\n    E[v_k] = 0, ~Cov[v_k,v_j] = R_k\\delta_{k,j}\n\\end{aligned}\n$$\n\n式中, $Q_k$ 为过程噪声方差矩阵; $R_k$ 为量测噪声方差阵.\n\n将非线性状态方程 $f(\\cdot)$ 在状态估计值 $\\hat{x}_{k-1}$ 附近展成一阶Taylor级数,可得\n\n$$\nf(x_{k-1})\\approx f(\\hat{x}{k-1})+\\dfrac{\\partial f}{\\partial x}\\biggl\\vert{\\hat{x}{k-1}},(x{k-1}-\\hat{x}{k-1}) + w{k-1}\n$$\n\n式中,\n\n$$\n\\begin{aligned}\n    \\frac{\\partial f}{\\partial x}\\bigg \\vert_{\\hat{x}_{k-1}} =\n    \\begin{bmatrix}\n     \\dfrac{\\partial f_1(\\cdot)}{\\partial x_1} \u0026 \\dfrac{\\partial f_1(\\cdot)}{\\partial x_1} \u0026 \\cdots \u0026 \\dfrac{\\partial f_1(\\cdot)}{\\partial x_1}\\\\\n     \\dfrac{\\partial f_2(\\cdot)}{\\partial x_2} \u0026 \\dfrac{\\partial f_2(\\cdot)}{\\partial x_2} \u0026 \\cdots \u0026 \\dfrac{\\partial f_2(\\cdot)}{\\partial x_2}\\\\\n     \\vdots \u0026 \\vdots \u0026  \u0026 \\vdots\\\\\n     \\dfrac{\\partial f_4(\\cdot)}{\\partial x_4} \u0026 \\dfrac{\\partial f_4(\\cdot)}{\\partial x_4} \u0026 \\cdots \u0026 \\dfrac{\\partial f_4(\\cdot)}{\\partial x_4}\n    \\end{bmatrix}\n\\end{aligned}\n$$\n\n同理,将非线性量测方程 $h_k(\\cdot)$ 在状态估计值 $\\hat{x}_{k|k-1}$ 附近展开成一阶Taylor级数,可得\n\n$$\nz_k \\approx \\hat{h}(\\hat{x}|)+\\frac{\\partial\\hat{h}}{\\partial x}-\\hat{x}_{k}|)\n$$\n\n式中,\n\n$$\n\\begin{aligned}\n    \\frac{\\partial h}{\\partial x}\\bigg \\vert_{\\hat{x}_{k|k-1}} =\n    \\begin{bmatrix}\n     \\dfrac{\\partial h_1(\\cdot)}{\\partial x_1} \u0026 \\dfrac{\\partial h_1(\\cdot)}{\\partial x_1} \u0026 \\cdots \u0026 \\dfrac{\\partial h_1(\\cdot)}{\\partial x_1}\\\\\n     \\dfrac{\\partial h_2(\\cdot)}{\\partial x_2} \u0026 \\dfrac{\\partial h_2(\\cdot)}{\\partial x_2} \u0026 \\cdots \u0026 \\dfrac{\\partial h_2(\\cdot)}{\\partial x_2}\\\\\n     \\vdots \u0026 \\vdots \u0026  \u0026 \\vdots\\\\\n     \\dfrac{\\partial h_4(\\cdot)}{\\partial x_4} \u0026 \\dfrac{\\partial h_4(\\cdot)}{\\partial x_4} \u0026 \\cdots \u0026 \\dfrac{\\partial h_4(\\cdot)}{\\partial x_4}\n    \\end{bmatrix}\n\\end{aligned}\n$$\n\n令\n\n$$\n    \\left.\\begin{array}{l}{{H_{k}=\\left.\\dfrac{\\partial h}{\\partial x}\\right|_{\\hat{x}_{k|k-1}}}}\\\\ {{\\left.\\zeta_{k}= h({\\hat{x}})-\\dfrac{\\partial h}{\\partial x}\\right{{x_{k|k-1}}}\\hat{x}_{k|k-1}}}\\end{array} \\right\\}\n$$\n\n则非线性量测方程的一阶线性化结果为:\n\n$$\n\\begin{aligned}\n    z_k = H_k x_k + \\zeta_k +v_k\n\\end{aligned}\n$$\n\n## 建模过程\n\n### 问题描述\n\n假设有三架无人机对移动目标进行协同定位,每架无人机上安装有被动雷达导引头,可提供高低角及方位角两种量测信息.请根据以下仿真条件,完成基于扩展卡尔曼滤波的协同定位滤波器设计及仿真分析.\n\n### 仿真条件\n\n1. 初始时刻,地面系下目标状态位置初值:\n    $p=[x, y, z]=[653.42,567.81,590.61] ~m$ ;\u003cbr\u003e\n    速度初值: $v=[v_x,v_y,v_z]=[89.93,61.87,84.41] ~m/s$ ;\u003cbr\u003e\n    加速度初值: $a=[a_x,a_y,a_z]~m/s^2$ ;\u003cbr\u003e\n    目标状态初值的误差:位置误差 $\\Delta p =[500,500,500]$ ;\u003cbr\u003e\n    速度误差 $\\Delta v = [50, 50, 50]$ ;\u003cbr\u003e\n    加速度 $\\Delta a = [5,5,5]$ (用于构造误差协方差矩阵初值).\n\n2. 目标运动模型选用\\\"当前\\\"统计模型,机动系数可选为 $\\dfrac{1}{60}$;\n\n3. 导引头测角噪声为零均值高斯噪声,标准差 $\\sigma = 0.5^{\\circ}$;\n\n4. 导引头测量帧频为 $10\\mathrm{HZ}$ ,共 $40\\mathbf{s}$;\n\n5. 不考虑无人机姿态变化,并假设无人机本体系与地面系重合.\n\n### \"当前\"统计模型\n\n以地理系为参考坐标系,选取地理系下目标运动的位置 $(x,y,z)$ ,速度 $(v_x,v_y,v_z)$ ,加速度 $(a_x,a_y,a_z)$ 为状态变量,有\n\n$$\n\\begin{aligned}\n    X = [x,y,z,v_x,v_y,v_z,a_x.a_y,a_z]\n\\end{aligned}\n$$\n\n根据\\\"当前\\\"统计模型建立的目标运动离散化方程为\n\n$$\n\\begin{aligned}\n    X_k = \\Phi_{k|k-1}X_{k-1} + U_{k-1}\\bar{a_k} + w_k\n\\end{aligned}\n$$\n\n其中,状态转移矩阵分别为\n\n$$\n\\begin{aligned}\n    \\Phi_{k|k-1}=\\left[\n        \\begin{array}{c c c}\n        {{I_{3\\times3}}}\u0026{{T\\cdot I_{3\\times3}}}\u0026{{\\frac{1}{\\alpha^{2}}\\left[-1+\\alpha\\cdot T+e^{-\\alpha T}\\right]\\cdot I_{3\\times3}}}\\\\ {{0_{3\\times3}}}\u0026{{I_{3\\times3}}}\u0026{{\\frac{1}{\\alpha}\\left[1-e^{-\\alpha T}\\right]\\cdot I_{3\\times3}}}\\\\ {{0_{3\\times3}}}\u0026{{0_{3\\times3}}}\u0026{{e^{-\\alpha T}\\cdot I_{3\\times3}}}\\end{array}\n        \\right] \\\\\n    U_{k|k-1}=\\left[\\begin{array}{c}{{\\left(\\displaystyle\\frac{T^{2}}{2}-\\frac{\\alpha T-1+e^{-\\alpha T}}{\\alpha^{2}}\\right)\\cdot I_{3\\times3}}}\\\\ {{\\alpha^{2}}}\\\\ {{\\left(T-\\frac{1-e^{-\\alpha T}}{\\alpha}\\right)I_{3\\times3}}}\\\\ {{(1-e^{-\\alpha T})I_{3\\times3}}}\\end{array}\\right]\n\\end{aligned}\n$$\n\n对于过程噪声协方差矩阵,为:\n\n$$\n\\begin{aligned}\n    Q = 2 \\alpha\n    \\begin{bmatrix}\n     q_{11}\\cdot \\sigma^2 \u0026 q_{12} \\cdot \\sigma^2\u0026 q_{13}\\cdot \\sigma^2\\\\\n     q_{21}\\cdot \\sigma^2 \u0026 q_{22}\\cdot \\sigma^2 \u0026 q_{23}\\cdot \\sigma^2\\\\\n     q_{31}\\cdot \\sigma^2 \u0026 q_{32}\\cdot \\sigma^2 \u0026 q_{33}\\cdot \\sigma^2\n    \\end{bmatrix}\n\\end{aligned}\n$$\n\n其中,\n\n$$\n    \\left \\{\n    \\begin{array}{lll}\n    q_{11} = \\dfrac{(2\\alpha^3T^3-6\\alpha^2T^2+6\\alpha T+3-12\\alpha T e^{-\\alpha*T}-3e^{-2\\alpha T})}{6\\alpha^5};\\\\\n    q_{12} = \\dfrac{\\alpha^2T^2-2\\alpha T+1-2 (1-\\alpha T) e^{-\\alpha T}+ e^{-2\\alpha T}}{2\\alpha^4};\\\\\n    q_{21} = q_{12};\\\\\n    q_{22} = \\dfrac{2\\alpha T-3+4 e^{-\\alpha T}-e^{-2\\alpha T}}{2\\alpha^3};\\\\\n    q_{13} = \\dfrac{1-2\\alpha T e^{-\\alpha T}-e^{-2\\alpha T}}{2\\alpha^3};\\\\\n    q_{31} = q_{13};\\\\\n    q_{23} = \\dfrac{1-2e^{-\\alpha T}+e^{-2\\alpha T}}{2\\alpha^3};\\\\\n    q_{32} = q_{23};\\\\\n    q_{33} = \\dfrac{1-e^{-2\\alpha T}}{2\\alpha};\n\\end{array}\n\\right.\n$$\n\n$$\n\\begin{aligned}\n    \\sigma ^2 =\n    \\begin{bmatrix}\n     \\sigma_x ^2 \u0026 0 \u0026 0\\\\\n     0 \u0026 \\sigma_y ^2 \u0026 0\\\\\n     0 \u0026 0 \u0026 \\sigma_z ^2\n\\end{bmatrix}\n\\end{aligned}\n$$\n\n对于加速度方差 $\\sigma$ 在每个轴上的分量如式表示\n\n$$\n\\begin{aligned}\n    \\sigma_{i}^{2}=\\left\\{\\begin{array}{l l}{{\\displaystyle\\frac{4-\\pi}{\\pi}[a_{\\mathrm{max}}-a]^{2}\\mathrm{~,~}0\u003ca\u003ca_{\\mathrm{max}}}}\\\\ {{\\displaystyle\\frac{4-\\pi}{\\pi}[a_{\\mathrm{-max}}+a]^{2}\\mathrm{~,~}a_{\\mathrm{-max}}\u003ca\u003c0}}\\end{array}\\right.\n    \\quad ,i = x, y,z\n\\end{aligned}\n$$\n\n在本文中,假设 $a_{max} = 15$, $a_{-max} = -15$ .\n\n### 系统量测方程\n\n无人机量测数据为量测高低角 $\\gamma$ 和量测方位角 $\\eta$ .三架无人机即测量三架无人机以及\n量测高低角/方位角定义:\n\n$$\n\\begin{aligned}\n    \\left\\{\\begin{array}{l}{{\\gamma_{i}=\\arcsin\\left(\\frac{\\displaystyle y_{r s,i}}{\\displaystyle\\sqrt{x_{r s,i}^{2}+y_{r s,i}^{2}+z_{r s,i}^{2}}}\\right)}} + v_{\\gamma,i}\\\\\n    {{\\displaystyle\\eta_{i}=\\arctan2\\left(-z_{r s,i},x_{r s,i}\\right) + v_{\\eta, i}}}\\end{array}\\right.\n\\end{aligned}\n$$\n\n无人机与目标之间的相对距离需要转化至地面坐标系上,所以有:\n\n$$\n\\begin{aligned}\n    \\left[\\begin{array}{c}{{x_{r s,i}}}\\\\ {{y_{r s,i}}}\\\\ {{z_{r s,i}}}\\\\ \\end{array}\\right]=C_{d}^{s_{i}}\\left[\\begin{array}{c}{{x-x_{s,i}}}\\\\ {{y-y_{s,i}}}\\\\ {{z-z_{s,i}}}\\end{array}\\right]\n\\end{aligned}\n$$\n\n所以,系统的量测方程为式:\n\n$$\n\\begin{aligned}\n    z =\n    \\left[\\begin{array}{c}\n     \\gamma_1\\\\\n     \\eta_1\\\\\n     \\gamma_2\\\\\n     \\eta_2\\\\\n     \\gamma_3\\\\\n     \\eta_3\n    \\end{array}\\right]\n\\end{aligned}\n$$\n\n其中 $[x~y~z]^T$ 为目标地面系下位置,  $[x_{s,t}, y_{s,t}, z_{s,t}]]^T$ 为第以架无人机地面系的位置, $C_d^{s_i}$ 为地面系到第  $i$ 架无人机本体系的坐标转换矩阵,在仿真条件5下为 $C_d^{s_i}$ 单位阵.\n\n### 仿真与分析\n\n在MATLAB中基于以上仿真条件进行时长40s的系统仿真,具体结果如图1 - 4所示.\n\n从图中可以看出,位置的收敛效果较好,在初始误差较大的情况下仅仅2秒后即可快速收敛;速度误差中x轴速度收敛效果较好,但是y轴和z轴尚未完全收敛,其中x轴和y轴误差在 $\\pm 20 m/s$ 以内,x轴误差则在 $\\pm 50 m/s$ 以内;加速度误差中收敛效果较差,其中x轴和y轴误差在 $\\pm$ 10以内,z轴误差则在 $\\pm$ 5以内.\n\n可以看出,在给定40s的时间范围内,应用扩展卡尔曼滤波,对系统状态进行最优最优估计,可以对目标的位置进行较好的估计,但是对速度和加速度的估计较差.\n\n\u003cimg src=\"fig/预测运动轨迹与真实轨迹三维.png\" alt=\"三维预测运动轨迹与真实轨迹\" style=\"zoom:20%;\" /\u003e\n\n\u003cimg src=\"fig/三轴位置跟踪曲线.png\" alt=\"三轴位置跟踪曲线\" style=\"zoom:20%;\" /\u003e\n\n\u003cimg src=\"fig/位置误差曲线.png\" alt=\"位置误差曲线\" style=\"zoom:20%;\" /\u003e\n\n\u003cimg src=\"fig/速度误差曲线.png\" alt=\"速度误差曲线\" style=\"zoom:20%;\" /\u003e\n\n\u003cimg src=\"fig/加速度误差曲线.png\" alt=\"加速度误差曲线\" style=\"zoom:20%;\" /\u003e\n\n## 附录\n\n就是文件中的代码~\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Ftsinglinrain%2Fekf_example","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Ftsinglinrain%2Fekf_example","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Ftsinglinrain%2Fekf_example/lists"}