我们只需要定义域对于我们的变量和一组承包商实现我们的约束。Codac的核心是承包商网络表示解算器。
只需几个步骤,问题就可以通过以下方式解决
定义变量(向量、轨迹)的初始域(框、管)
从图书馆提供的现有运营商目录中选取承包商
将承包商和域添加到承包商网络
让承包商网络解决问题
获得一组可靠的可行变量
例如。
让我们考虑一下机器人的测距定位问题。机器人由状态向量描述\(\mathbf{x}=\{x_1,x_2,\psi,\vartheta\}^\internal)描述其位置、方向和速度。它在三个地标之间发展\(\mathbf{b} _1个\),\(\mathbf{b} _2\),\(\mathbf{b} _3个\)并测量距离\(y_i\)从这些方面来看。
该问题由经典状态方程定义:
\[\begin{split}\left\{\begin}数组}{l}\点{\mathbf{x}}(t)=\mathbf1}\big\\y_i=g\大(\mathbf{x}(ti),\mathbf{b} _ i\大)\右端{数组}。\结束{拆分}\]
哪里\(\mathbf{u}(t)\)是系统的输入,已知有一些不确定性。\(\mathbf{f}\)和\(g)是非线性函数。
我们有三个随时间变化的变量:轨迹\(\mathbf{x}(t)\),\(\mathbf{v}(t)=\dot{\mathbf{x}}(t)\),\(\mathbf{u}(t)\)。我们定义了三个管来封装它们:
日期 = 0.01 #管精度的时间步长
T域 = 间隔(0, 三) #时间极限[t0,tf]=[0,3]
x个 = 管向量(TubeVector)(T域, 日期, 4) #状态向量4d管
v(v) = 管向量(TubeVector)(T域, 日期, 4) #态导数4d管
u个 = 管向量(TubeVector)(T域, 日期, 2) #系统输入的2d管
浮动 日期 = 0.01; //管精度的时间步长
间隔 T域(0, 三); //时间限制[t_0,t_f]=[0,3]
管向量(TubeVector) x个(T域, 日期, 4); //状态向量4d管
管向量(TubeVector) v(v)(T域, 日期, 4); //态导数4d管
管向量(TubeVector) u个(T域, 日期, 2); //系统输入的2d管
我们假设我们对标题进行了测量\(磅/平方英寸(吨))和速度\(\vartheta(t)\),一些有界不确定性由区间定义\([e_\psi]=[-0.01,0.01]\),\([e_\vartheta]=[-0.01,0.01]\):
x个[2] = 管子(测量的psi, 日期)。膨胀(0.01) #measuredpsi是一组测量值
x个[三] = 管子(测量的速度, 日期)。膨胀(0.01)
x个[2] = 管子(测量的psi, 日期).膨胀(0.01); //measuredpsi是一组测量值
x个[三] = 管子(测量的速度, 日期).膨胀(0.01);
最后,我们定义了三个单距离观测值的区域\((ti,y_i)\)以及地标的位置。距离\(y_i\)以区间为界\([e_y]=[-0.1,0.1]\)。
(_y) = 间隔(-0.1,0.1)
年 = [间隔(1.9+(_y)), 间隔(3.6+(_y)),\#一组距离观测值
间隔(2.8+(_y))]
b条 = [[8,三][0,5][-2,1]] #三个二维地标的位置
吨 = [0.3, 1.5, 2] #测量次数
间隔 (_y)(-0.1,0.1);
矢量<间隔> 年 = {1.9+(_y), 3.6+(_y), 2.8+(_y)}; //一组距离观测值
矢量<矢量> b条 = {{8,三}, {0,5}, {-2,1}}; //三个二维地标的位置
矢量<双重的> 吨 = {0.3, 1.5, 2}; //测量次数
距离函数\(g(\mathbf{x},\mathbf{b})\)机器人和地标之间对应于Ctc医生
图书馆提供的承包商。进化函数\(\mathbf{f}(\mathbf{x},\mathbf{u})=\大(x4\cos(x3),x4\sin(x3,u1,u2\big)\)可由定制承包商处理:
ctc_f = Ctc函数(
功能(“v[4]”, “x[4]”, “u[2]”,
“(v[0]-x[3]*cos(x[2]);v[1]-x[3]*sin(x[2]);v[2]-u[0];v[3]-u[1])”))
Ctc函数 ctc_f公司(
功能(“v[4]”, “x[4]”, “u[2]”,
“(v[0]-x[3]*cos(x[2]);v[1]-x[3]*sin(x[2]);v[2]-u[0];v[3]-u[1])”));
第三步。
将承包商与相关域一起添加到网络中非常简单:
中国 = 承包商网络() #创建网络
中国。添加(ctc_f, [v(v), x个, u个]) #添加f约束
对于 我 在里面 范围 (0,伦恩(年)): #我们添加了观察者。每个距离测量的约束
第页 = 中国。创建期限(间隔向量(4)) #中间。变量(ti状态)
#距离约束:ti处的状态与第i个信标位置之间的关系
中国。添加(ctc公司。距离, [中国。子向量(第页,0,1), b条[我], 年[我]])
#评估约束:t_i处的状态与[t_0,t_f]上的所有状态之间的关系
中国。添加(ctc公司。评估, [吨[我], 第页, x个, v(v)])
承包商网络 中国; //创建网络
中国。添加(ctc_f, {v(v), x个, u个}); //添加f约束
对于(整数 我 = 0 ; 我 < 三 ; 我++) //我们添加了observ。每个距离测量的约束
{
间隔向量& 第页 = 中国。创建期限(间隔向量(4)); //中间。变量(ti状态)
//距离约束:ti处的状态与第i个信标位置之间的关系
中国。添加(ctc公司::距离, {中国。子向量(第页,0,1), b条[我], 年[我]});
//评估约束:t_i处的状态与[t_0,t_f]上的所有状态之间的关系
中国。添加(ctc公司::评估, {吨[我], 第页, x个, v(v)});
}
第五步。
获得一组可靠的可行位置:用蓝色表示的管子。三个黄色机器人展示了观察的三个瞬间。白线是未知的真相。
你只是在不了解初始条件的情况下求解了一个非线性状态估计。
在本教程和该库的examples文件夹中,您将发现更多高级问题,如同步本地化和映射(SLAM)、数据关联问题或延迟系统。