zero_velocity_updater.hpp

// Copyright (C) 2023 Alessandro Fornasier.
// Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the Apache License, Version 2.0
// (the "License"); you may not use this file except in compliance with the
// License. You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations
// under the License.
//
// You can contact the authors at <alessandro.fornasier@ieee.org>
 
#ifndef ZERO_VELOCITY_UPDATER_HPP
#define ZERO_VELOCITY_UPDATER_HPP
 
#include "msceqf/filter/checker/checker.hpp"
#include "msceqf/state/state.hpp"
 
namespace msceqf
{
class ZeroVelocityUpdater
{
 public:
  ZeroVelocityUpdater(const ZeroVelocityUpdaterOptions& opts, const Checker& checker);

  void setMotion();

  void setMeasurement(const SE23& y);

  [[nodiscard]] bool isActive(const Tracks& tracks);

  [[nodiscard]] bool zvUpdate(MSCEqFState& X, const SystemState& xi0) const;
 
 private:
  ZeroVelocityUpdaterOptions opts_;  
 
  const Checker& checker_;  // The MSCEqF checker
 
  SE23 y_;  
 
  bool motion_;  
};
 
}  // namespace msceqf
 
#endif  // ZERO_VELOCITY_UPDATER_HPP