文章目录
前言
一个人可以走的更快，一群人才能走的更远，交流学习加qq:2096723956
更多保姆级PX4+ROS学习视频：b23.tv/ZeUDKqy
本文主要介绍PX4中基于卡尔曼滤波的位置解算（local_position_estimator），该模块通常与四元数互补滤波配合使用．
PX4固件版本：1.13.0
代码位置
一、组合导航系统模型
INS/GPS组合导航系统一般有两种信息融合方式:速度、位置组合(松耦合)和伪距、伪距率组合(紧耦合)，PX4采用前一种方式，这种方法的测量模型是线性的，可以用标准卡尔曼滤波器进行信息的融合。惯性导航系统(INS)以牛顿力学为基础，首先由姿态解算得到姿态矩阵，建立起导航坐标系。然后利用加速度计来测量无人机的实时加速度，将实时加速度通过姿态矩阵转换到导航坐标系中，经过一次积分运算得到实时速度，再一次积分运算得出实时位置.
二、代码解析
1. 主程序
void BlockLocalPositionEstimator::Run()
{
if (should_exit())
{
_sensors_sub.unregisterCallback();
exit_and_cleanup();
return;
}
判断是否需要通过command命令设置全球位置的初始位置。
if (_vehicle_command_sub.updated())
{
vehicle_command_s vehicle_command;
if (_vehicle_command_sub.update(&vehicle_command))
{
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN)
{
const double latitude = vehicle_command.param5;
const double longitude = vehicle_command.param6;
const float altitude = vehicle_command.param7;
_global_local_proj_ref.initReference(latitude, longitude, vehicle_command.timestamp);
_global_local_alt0 = altitude;
PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
}
}
}
imu没有更新则返回
sensor_combined_s imu;
if (!_sensors_sub.update(&imu))
{
return;
}
计算两次运行的时间间隔dt
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
未解锁时，寻找是否有测据传感器
_sub_armed.update();
bool armedState = _sub_armed.get().armed;
不同的测据传感器发布的话题都是distance_sensor，采用的是一个多重发布的方式，因此这里订阅时，也是遍历四个这个话题的四个接口，如果没有找到雷达或者声呐发布的消息，则进行寻找
if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr))
{
遍历四个接口
for (size_t i = 0; i < N_DIST_SUBS; i++)
{
如果找到雷达或者声呐消息，跳出循环
auto *s = _dist_subs[i];
if (s == _sub_lidar || s == _sub_sonar)
{
continue;
}
如果话题更新，则开始寻找
if (s->update())
{
时间戳为0则退出
if (s->get().timestamp == 0)
{
continue;
}
判断找到雷达
if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_lidar == nullptr)
{
_sub_lidar = s;
mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i);
}
判断找到声呐
else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_sonar == nullptr)
{
_sub_sonar = s;
mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %zu", msg_label, i);
}
}
}
}
更新最新的解锁状态
_lastArmedState = armedState;
传感器数据可用标志
const bool paramsUpdated = _parameter_update_sub.updated();
_baroUpdated = false;
if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.update())
{
if (_sub_airdata.get().timestamp != _timeStampLastBaro)
{
_baroUpdated = true;
_timeStampLastBaro = _sub_airdata.get().timestamp;
}
}
_flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.update();
_gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.update();
_visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.update();
_mocapUpdated = _sub_mocap_odom.update();
_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->update();
_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->update();
_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
bool targetPositionUpdated = _sub_landing_target_pose.update();
跟新姿态和姿态角速率
_sub_att.update();
_sub_angular_velocity.update();
更新参数
if (paramsUpdated)
{
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
SuperBlock::updateParams();
ModuleParams::updateParams();
updateSSParams();
}
判断vx，vy是否可用，vx，vy的最大值要小于参数LPE_VXY_PUB的平方，即为可用
bool vxy_stddev_ok = false;
if (math::max(m_P(X_vx, X_vx), m_P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get())
{
vxy_stddev_ok = true;
}
如果在估计XY的时候GPS数据超时，则将EST_XY取反并置位，表示不估计XY方向的状态。_estimatorInitialized是一个标志量，有EST_XY、EST_Z、EST_TZ三个标志与其进行位运算，其中EST_TZ表示的是地形高度，如下：
EST_XY = 1 << 0, EST_Z = 1 << 1, EST_TZ = 1 << 2,
if (_estimatorInitialized & EST_XY)
{
// if valid and gps has timed out, set to not valid
if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS))
{
_estimatorInitialized &= ~EST_XY;
}
}
如果存在下面的额传感器则估计XY
else
{
if (vxy_stddev_ok)
{
if (!(_sensorTimeout & SENSOR_GPS)
|| !(_sensorTimeout & SENSOR_FLOW)
|| !(_sensorTimeout & SENSOR_VISION)
|| !(_sensorTimeout & SENSOR_MOCAP)
|| !(_sensorTimeout & SENSOR_LAND)
|| !(_sensorTimeout & SENSOR_LAND_TARGET)
)
{
_estimatorInitialized |= EST_XY;
}
}
}
判断Z是否可用，并判断是否进行Z方向的估计。同上
bool z_stddev_ok = sqrtf(m_P(X_z, X_z)) < _param_lpe_z_pub.get();
if (_estimatorInitialized & EST_Z)
{
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO))
{
_estimatorInitialized &= ~EST_Z;
}
}
else
{
if (z_stddev_ok)
{
_estimatorInitialized |= EST_Z;
}
}
判断地形高度是否可用，并判断是否进行地形高度的估计。同上
bool tz_stddev_ok = sqrtf(m_P(X_tz, X_tz)) < _param_lpe_z_pub.get();
if (_estimatorInitialized & EST_TZ)
{
if (!tz_stddev_ok)
{
_estimatorInitialized &= ~EST_TZ;
}
}
else
{
if (tz_stddev_ok)
{
_estimatorInitialized |= EST_TZ;
}
}
判断传感器是否超时
checkTimeouts();
一共有下面这个传感器，判断当前时间减去最新的传感器时间是否大于超时时间
void BlockLocalPositionEstimator::checkTimeouts() {
baroCheckTimeout();
gpsCheckTimeout();
lidarCheckTimeout();
flowCheckTimeout();
sonarCheckTimeout();
visionCheckTimeout();
mocapCheckTimeout();
landCheckTimeout();
landingTargetCheckTimeout(); }
如果GPS的初始位置没有初始化，则从参数LPE_LAT和LPE_LON里面获取初始位置
if (!_map_ref.isInitialized() && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get())
{
_map_ref.initReference(
(double)_param_lpe_lat.get(),
(double)_param_lpe_lon.get());
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin));
}
// reinitialize x if necessary
如果状态向量_x中有超出限制的值，将状态向量_x重置为0
bool reinit_x = false;
for (size_t i = 0; i < n_x; i++)
{
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i)))
{
reinit_x = true;
mavlink_log_info(&mavlink_log_pub, "%sreinit x, x(%zu) not finite", msg_label, i);
break;
}
}
if (reinit_x)
{
for (size_t i = 0; i < n_x; i++)
{
_x(i) = 0;
}
mavlink_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
}
判断状态协方差矩阵是否需要重置，如果有值超限或者对角线元素存在负数则重置
bool reinit_P = false;
for (size_t i = 0; i < n_x; i++)
{
for (size_t j = 0; j <= i; j++)
{
if (!PX4_ISFINITE(m_P(i, j)))
{
mavlink_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) not finite", msg_label, i, j);
reinit_P = true;
}
if (i == j)
{
// make sure diagonal elements are positive
if (m_P(i, i) <= 0)
{
mavlink_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) negative", msg_label, i, j);
reinit_P = true;
}
}
else
{
// copy elememnt from upper triangle to force
// symmetry
m_P(j, i) = m_P(i, j);
}
if (reinit_P)
{
break;
}
}
if (reinit_P)
{
break;
}
}
if (reinit_P)
{
initP();
}
状态预测，见第二节
predict(imu);
利用传感器的观测值进行修正，如果传感器数据超时，则重新初始化传感器，否则进行修正，具体的修正代码见第三节。
GPS
if (_gpsUpdated)
{
if (_sensorTimeout & SENSOR_GPS)
{
gpsInit();
}
else
{
gpsCorrect();
}
}
气压计
if (_baroUpdated)
{
if (_sensorTimeout & SENSOR_BARO)
{
baroInit();
}
else
{
baroCorrect();
}
}
雷达
if (_lidarUpdated)
{
if (_sensorTimeout & SENSOR_LIDAR)
{
lidarInit();
}
else
{
lidarCorrect();
}
}
声纳
if (_sonarUpdated)
{
if (_sensorTimeout & SENSOR_SONAR)
{
sonarInit();
}
else
{
sonarCorrect();
}
}
光流
if (_flowUpdated)
{
if (_sensorTimeout & SENSOR_FLOW)
{
flowInit();
}
else
{
flowCorrect();
}
}
视觉
if (_visionUpdated)
{
if (_sensorTimeout & SENSOR_VISION)
{
visionInit();
}
else
{
visionCorrect();
}
}
mocap(动捕)
if (_mocapUpdated)
{
if (_sensorTimeout & SENSOR_MOCAP)
{
mocapInit();
}
else
{
mocapCorrect();
}
}
if (_landUpdated)
{
if (_sensorTimeout & SENSOR_LAND)
{
landInit();
}
else
{
landCorrect();
}
}
if (targetPositionUpdated)
{
if (_sensorTimeout & SENSOR_LAND_TARGET)
{
landingTargetInit();
}
else
{
landingTargetCorrect();
}
}
发布本地位置、估计器状态、协方差、状态标志等数据
if (_altOriginInitialized)
{
// update all publications if possible
publishLocalPos();
publishOdom();
publishEstimatorStatus();
_pub_innov.get().timestamp_sample = _timeStamp;
_pub_innov.get().timestamp = hrt_absolute_time();
_pub_innov.update();
_pub_innov_var.get().timestamp_sample = _timeStamp;
_pub_innov_var.get().timestamp = hrt_absolute_time();
_pub_innov_var.update();
if ((_estimatorInitialized & EST_XY) && (_map_ref.isInitialized() || _param_lpe_fake_origin.get()))
{
publishGlobalPos();
}
}
状态传播延时更新
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP))
{
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
2.状态预测
void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu)
{
将姿态四元数转换为旋转矩阵
_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
获取加速度
Vector3f a(imu.accelerometer_m_s2);
将加速度数据从机体坐标系转换到地理坐标系
_u = _R_att * a;
无人机采用的是NED坐标系，Z轴正方向是指向地面的，当无人机静止时，加速度值是负的重力加速度，所以需要加上重力加速度抵消重力加速度的影响，得到运动加速度。
_u(U_az) += CONSTANTS_ONE_G; // add g
// update state space based on new states
updateSSStates();
连续时间卡尔曼滤波，利用四阶龙格库塔进行状态预测
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
状态方程如下：
判断是否进行水平方向位置速度的融合
if (!(_estimatorInitialized & EST_XY))
{
dx(X_x) = 0;
dx(X_vx) = 0;
dx(X_y) = 0;
dx(X_vy) = 0;
}
判断是否进行垂直方向位置的融合
if (!(_estimatorInitialized & EST_Z))
{
dx(X_z) = 0;
}
判断是否进行地形高度的融合
if (!(_estimatorInitialized & EST_TZ))
{
dx(X_tz) = 0;
}
// saturate bias
float bx = dx(X_bx) + _x(X_bx);
float by = dx(X_by) + _x(X_by);
float bz = dx(X_bz) + _x(X_bz);
if (std::abs(bx) > BIAS_MAX)
{
bx = BIAS_MAX * bx / std::abs(bx);
dx(X_bx) = bx - _x(X_bx);
}
if (std::abs(by) > BIAS_MAX)
{
by = BIAS_MAX * by / std::abs(by);
dx(X_by) = by - _x(X_by);
}
if (std::abs(bz) > BIAS_MAX)
{
bz = BIAS_MAX * bz / std::abs(bz);
dx(X_bz) = bz - _x(X_bz);
}
// propagate
_x += dx;
Matrix<float, n_x, n_x> dP = (m_A * m_P + m_P * m_A.transpose() +
m_B * m_R * m_B.transpose() + m_Q) * getDt();
// covariance propagation logic
for (size_t i = 0; i < n_x; i++)
{
if (m_P(i, i) > P_MAX)
{
// if diagonal element greater than max, stop propagating
dP(i, i) = 0;
for (size_t j = 0; j < n_x; j++)
{
dP(i, j) = 0;
dP(j, i) = 0;
}
}
}
m_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu)
{
将姿态四元数转换为旋转矩阵
_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
获取加速度
Vector3f a(imu.accelerometer_m_s2);
将加速度数据从机体坐标系转换到地理坐标系
_u = _R_att * a;
去除重力加速度，得到无人机的运动加速度
_u(U_az) += CONSTANTS_ONE_G;
// update state space based on new states
updateSSStates();
连续时间卡尔曼滤波，利用四阶龙格库塔进行状态预测
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
状态方程如下：
如果没有水平方向的位置则不进行水平方向位置的融合
if (!(_estimatorInitialized & EST_XY)) {
dx(X_x) = 0;
dx(X_vx) = 0;
dx(X_y) = 0;
dx(X_vy) = 0;
}
如果没有垂直方向的位置则不进行垂直方向位置的融合
if (!(_estimatorInitialized & EST_Z)) {
dx(X_z) = 0;
}
如果没有地形数据则不进行地形高度的融合
if (!(_estimatorInitialized & EST_TZ)) {
dx(X_tz) = 0;
}
漂移量抗饱和，先计算出bx、by、bz，然后判断如果大于BIAS_MAX，则将dx中的bx、by、bz进行限幅，bx = BIAS_MAX * bx / std::abs(bx)求出的是BIAS_MAX，符号和bx 相同，将 bx - _x(X_bx)赋值为偏移量，这样在后面进行状态更新时再加上_x(X_bx)，更新后的偏移量即为最大偏移量BIAS_MAX
float bx = dx(X_bx) + _x(X_bx);
float by = dx(X_by) + _x(X_by);
float bz = dx(X_bz) + _x(X_bz);
if (std::abs(bx) > BIAS_MAX) {
bx = BIAS_MAX * bx / std::abs(bx);
dx(X_bx) = bx - _x(X_bx);
}
if (std::abs(by) > BIAS_MAX) {
by = BIAS_MAX * by / std::abs(by);
dx(X_by) = by - _x(X_by);
}
if (std::abs(bz) > BIAS_MAX) {
bz = BIAS_MAX * bz / std::abs(bz);
dx(X_bz) = bz - _x(X_bz);
}
更新状态向量预测值
_x += dx;
根据卡尔曼滤波公式预测协方差矩阵m_P
Matrix<float, n_x, n_x> dP = (m_A * m_P + m_P * m_A.transpose() +
m_B * m_R * m_B.transpose() + m_Q) * getDt();
// covariance propagation logic
for (size_t i = 0; i < n_x; i++) {
if (m_P(i, i) > P_MAX) {
// if diagonal element greater than max, stop propagating
dP(i, i) = 0;
for (size_t j = 0; j < n_x; j++) {
dP(i, j) = 0;
dP(j, i) = 0;
}
}
}
m_P += dP;
低通滤波
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
3.观测修正
以GPS为例。
void BlockLocalPositionEstimator::gpsCorrect()
{
// measure
将GPS数据赋值给全球位置y_global
Vector<double, n_y_gps> y_global;
if (gpsMeasure(y_global) != OK) { return; }
将全球经纬度转换成本地XYZ坐标
double lat = y_global(Y_gps_x);
double lon = y_global(Y_gps_y);
float alt = y_global(Y_gps_z);
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltOrigin);
map_projection_project(&_map_ref, lat, lon, &px, &py);
赋值本地位置
Vector<float, n_y_gps> y;
y.setZero();
y(Y_gps_x) = px;
y(Y_gps_y) = py;
y(Y_gps_z) = pz;
y(Y_gps_vx) = y_global(Y_gps_vx);
y(Y_gps_vy) = y_global(Y_gps_vy);
y(Y_gps_vz) = y_global(Y_gps_vz);
GPS观测矩阵，和状态向量相关的值之间就是简单的一比一关系
Matrix<float, n_y_gps, n_x> C;
C.setZero();
C(Y_gps_x, X_x) = 1;
C(Y_gps_y, X_y) = 1;
C(Y_gps_z, X_z) = 1;
C(Y_gps_vx, X_vx) = 1;
C(Y_gps_vy, X_vy) = 1;
C(Y_gps_vz, X_vz) = 1;
GPS协方差矩阵
SquareMatrix<float, n_y_gps> R;
R.setZero();
如果设置了GPS协方差参数，使用设置的参数
float var_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get();
float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get();
float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get();
float var_vz = _param_lpe_gps_vz.get() * _param_lpe_gps_vz.get();
如果实际GPS的精度因子大于参数值，使用精度因子的平方作为协方差
if (_sub_gps.get().eph > _param_lpe_gps_xy.get()) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
if (_sub_gps.get().epv > _param_lpe_gps_z.get()) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
float gps_s_stddev = _sub_gps.get().s_variance_m_s;
if (gps_s_stddev > _param_lpe_gps_vxy.get()) {
var_vxy = gps_s_stddev * gps_s_stddev;
}
if (gps_s_stddev > _param_lpe_gps_vz.get()) {
var_vz = gps_s_stddev * gps_s_stddev;
}
赋值协方差矩阵
R(0, 0) = var_xy;
R(1, 1) = var_xy;
R(2, 2) = var_z;
R(3, 3) = var_vxy;
R(4, 4) = var_vxy;
R(5, 5) = var_vz;
计算上一次的观测值x0
uint8_t i_hist = 0;
if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
计算残差
Vector<float, n_y_gps> r = y - C * x0;
计算残差协方差
Matrix<float, n_y_gps, n_y_gps> S = C * m_P * C.transpose() + R;
发布
_pub_innov.get().gps_hpos[0] = r(0);
_pub_innov.get().gps_hpos[1] = r(1);
_pub_innov.get().gps_vpos = r(2);
_pub_innov.get().gps_hvel[0] = r(3);
_pub_innov.get().gps_hvel[1] = r(4);
_pub_innov.get().gps_vvel = r(5);
_pub_innov_var.get().gps_hpos[0] = S(0, 0);
_pub_innov_var.get().gps_hpos[1] = S(1, 1);
_pub_innov_var.get().gps_vpos = S(2, 2);
_pub_innov_var.get().gps_hvel[0] = S(3, 3);
_pub_innov_var.get().gps_hvel[1] = S(4, 4);
_pub_innov_var.get().gps_vvel = S(5, 5);
残差协方差求逆
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
故障检测
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
// artifically increase beta threshhold to prevent fault during landing
float beta_thresh = 1e2f;
if (beta / BETA_TABLE[n_y_gps] > beta_thresh) {
if (!(_sensorFault & SENSOR_GPS)) {
mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
double(r(0) * r(0) / S_I(0, 0)), double(r(1) * r(1) / S_I(1, 1)), double(r(2) * r(2) / S_I(2, 2)),
double(r(3) * r(3) / S_I(3, 3)), double(r(4) * r(4) / S_I(4, 4)), double(r(5) * r(5) / S_I(5, 5)));
_sensorFault |= SENSOR_GPS;
}
} else if (_sensorFault & SENSOR_GPS) {
_sensorFault &= ~SENSOR_GPS;
mavlink_log_info(&mavlink_log_pub, "[lpe] GPS OK");
}
卡尔曼滤波观测修正
计算卡尔曼增益
Matrix<float, n_x, n_y_gps> K = m_P * C.transpose() * S_I;
计算修正量
Vector<float, n_x> dx = K * r;
修正状态向量
_x += dx;
协方差矩阵更新
m_P -= K * C * m_P;
}
