matlab代码sqrt
Optimal-Estimate-BigHomework
姓名:刘振博
学号:2019201920
完成工作
一维状态量的KF仿真
二维状态量的EKF仿真
应用EKF实现2D-SLAM
一维状态量的KF仿真
系统建模:
x+
=
F_x
*x
+
F_u
*
u
+
F_n
*
n
y
=
H
*
x
+
v
其中:
F_x
=
1;
F_u
=
1;
F_n
=
1;
u
=
1;
H
=
0.5;
Q
=
1;
R
=
1;
状态先验:
x
=
0;
P
=
1e4;
仿真初值:
X
=
7;
仿真结果:
二维状态量的EKF仿真
系统模型:
x+
=
f
(
x,
u,
n
)
y
=
h
(
x
)
+
v
系统定义:
x
=
[px
py
vx
vy]'
y
=
[d,
a]'
u
=
[ax,
ay]'
n
=
[nx,
ny]'
v
=
[vd,
va]'
px+
=
px
+
vx*dt
py+
=
py
+
vy*dt
vx+
=
vx
+
ax*dt
+
nx
vy+
=
vy
+
ay*dt
+
ny
d
=
sqrt(px^2
+
py^
2023-02-21 11:51:09
19.65MB
系统开源
1