/*
|
* Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and
|
* its subsidiaries and affiliates (collectly called MKSEMI).
|
*
|
* All rights reserved.
|
*
|
* Redistribution and use in source and binary forms, with or without
|
* modification, are permitted provided that the following conditions are met:
|
*
|
* 1. Redistributions of source code must retain the above copyright notice,
|
* this list of conditions and the following disclaimer.
|
*
|
* 2. Redistributions in binary form, except as embedded into an MKSEMI
|
* integrated circuit in a product or a software update for such product,
|
* must reproduce the above copyright notice, this list of conditions and
|
* the following disclaimer in the documentation and/or other materials
|
* provided with the distribution.
|
*
|
* 3. Neither the name of MKSEMI nor the names of its contributors may be used
|
* to endorse or promote products derived from this software without
|
* specific prior written permission.
|
*
|
* 4. This software, with or without modification, must only be used with a
|
* MKSEMI integrated circuit.
|
*
|
* 5. Any software provided in binary form under this license must not be
|
* reverse engineered, decompiled, modified and/or disassembled.
|
*
|
* THIS SOFTWARE IS PROVIDED BY MKSEMI "AS IS" AND ANY EXPRESS OR IMPLIED
|
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
* MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
* DISCLAIMED. IN NO EVENT SHALL MKSEMI OR CONTRIBUTORS BE LIABLE FOR ANY
|
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
|
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
*/
|
|
#ifndef MK8000_KF_TOP_H
|
#define MK8000_KF_TOP_H
|
#include "mk_common.h"
|
|
#define ANGLE_AMBIGUITY_THRES 280
|
|
enum KF_CHANNEL_TYPE_T
|
{
|
KF_CHANNEL_TYPE_RANGING = 0,
|
KF_CHANNEL_TYPE_ANGLE = 1,
|
KF_CHANNEL_TYPE_ANGLE_POSITIVE = 2,
|
};
|
|
typedef struct
|
{
|
float dt;
|
float Q; // covariance of the process noise
|
float rR; // covariance of the obervation noise for ranging
|
float aR; // covariance of the obervation noise for azimuth & elevation
|
float r_v_max; // ranging velocity limit for state estimation
|
float a_v_max; // azimuth/elevation velocity limit for state estimation
|
uint16_t r_vlimit_opt; // enable ranging velocity limitation
|
uint16_t a_vlimit_opt; // enable azimuth/elevation velocity limitation
|
} kf_params_t;
|
|
extern uint8_t pdoa_3d_ant_layout_get(void);
|
|
typedef struct
|
{
|
int row, col;
|
float *element;
|
} Mat;
|
|
typedef struct
|
{
|
Mat H;
|
Mat F;
|
Mat G;
|
Mat P_t_tm1;
|
Mat X_t_tm1;
|
Mat K_t;
|
Mat X_t_t;
|
float last_post_range;
|
float v_max;
|
float v_max_default;
|
float q_variance;
|
uint16_t v_state;
|
uint16_t v_max_cnt;
|
} kf_channel_env_t;
|
|
typedef struct
|
{
|
float mat_H[1 * 2];
|
float mat_P_t_tm1[2 * 2];
|
float mat_X_t_tm1[2 * 1];
|
float mat_K_t[2 * 1];
|
float mat_X_t_t[2 * 1];
|
float mat_F[2 * 2];
|
float mat_G[2 * 1];
|
} kf_channel_mat_value_t;
|
|
void MK8000_kf_config(kf_params_t params, kf_channel_env_t *channel_env, kf_channel_mat_value_t *channel_mat_value, size_t cache_len);
|
|
void MK8000_kf_processor(float data_meas, uint16_t channel, enum KF_CHANNEL_TYPE_T type, int8_t do_init, float *post_data);
|
|
#endif // MK8000_KF_TOP_H
|
