What is Mahalanobis distance? 马氏距离

本文详细解析了马氏距离的概念,包括其几何意义,如何通过Cholesky分解将变量从无相关转变为具有特定协方差结构,以及逆Cholesky变换用于消除变量间的相关性。此外,文章介绍了Cholesky分解在生成正态分布数据和解决涉及协方差矩阵的线性系统中的应用。

https://blogs.sas.com/content/iml/2012/02/15/what-is-mahalanobis-distance.html

https://blogs.sas.com/content/iml/2012/02/08/.html

https://stats.stackexchange.com/questions/62092/bottom-to-top-explanation-of-the-mahalanobis-distance
上面三个网页对马氏距离解释的很好

A variance-covariance matrix expresses linear relationships between variables. Given the covariances between variables, did you know that you can write down an invertible linear transformation that “uncorrelates” the variables? Conversely, you can transform a set of uncorrelated variables into variables with given covariances. The transformation that works this magic is called the Cholesky transformation; it is represented by a matrix that is the “square root” of the covariance matrix.

The Square Root Matrix
Given a covariance matrix, Σ, it can be factored uniquely into a product Σ=UTU, where U is an upper triangular matrix with positive diagonal entries and the superscript denotes matrix transpose. The matrix U is the Cholesky (or “square root”) matrix. Some people (including me) prefer to work with lower triangular matrices. If you define L=UT, then Σ=LLT. This is the form of the Cholesky decomposition that is given in Golub and Van Loan (1996, p. 143). Golub and Van Loan provide a proof of the Cholesky decomposition, as well as various ways to compute it.

Geometrically, the Cholesky matrix transforms uncorrelated variables into variables whose variances and covariances are given by Σ. In particular, if you generate p standard normal variates, the Cholesky transformation maps the variables into variables for the multivariate normal distribution with covariance matrix Σ and centered at the origin (denoted MVN(0, Σ)).

The Cholesky Transformation: The Simple Case
Let’s see how the Cholesky transofrmation works in a very simple situation. Suppose that you want to generate multivariate normal data that are uncorrelated, but have non-unit variance. The covariance matrix for this situation is the diagonal matrix of variances: Σ = diag(σ21,…,σ2p). The square root of Σ is the diagonal matrix D that consists of the standard deviations: Σ = DTD where D = diag(σ1,…,σp).

Geometrically, the D matrix scales each coordinate direction independently of the other directions. This is shown in the following image. The X axis is scaled by a factor of 3, whereas the Y axis is unchanged (scale factor of 1). The transformation D is diag(3,1), which corresponds to a covariance matrix of diag(9,1). If you think of the circles in the top image as being probability contours for the multivariate distribution MVN(0, I), then the bottom shows the corresponding probability ellipses for the distribution MVN(0, D).

在这里插入图片描述

Geometry of transformation: A diagonal matrix transforms uncorrelated standardized variables to uncorrelated scaled variables. Shown for bivariate data.
The General Cholesky Transformation Correlates Variables
In the general case, a covariance matrix contains off-diagonal elements. The geometry of the Cholesky transformation is similar to the “pure scaling” case shown previously, but the transformation also rotates and shears the top image.

The following graph shows the geometry of the transformation in terms of the data and in terms of probability ellipses. The top graph is a scatter plot of the X and Y variables. Notice that they are uncorrelated and that the probability ellipses are circles. The bottom graph is a scatter plot of the Z and W variables. Notice that they are correlated and the probability contours are ellipses that are tilted with respect to the coordinate axes. The bottom graph is the transformation under L of points and circles in the top graph.

在这里插入图片描述
The Inverse Cholesky Transformation Uncorrelates Variables
You might wonder: Can you go the other way? That is, if you start with correlated variables, can you apply a linear transformation such that the transformed variables are uncorrelated? Yes, and it’s easy to guess the transformation that works: it is the inverse of the Cholesky transformation!

Suppose that you generate multivariate normal data from MVN(0,Σ). You can “uncorrelate” the data by transforming the data according to L^-1

Success! The covariance matrix is essentially the identity matrix. The inverse Cholesky transformation “uncorrelates” the variables.

The TRISOLV function, which uses back-substitution to solve the linear system, is extremely fast. Anytime you are trying to solve a linear system that involves a covariance matrix, you should try to solve the system by computing the Cholesky factor of the covariance matrix, followed by back-substitution.

In summary, you can use the Cholesky factor of a covariance matrix in several ways:

To generate multivariate normal data with a given covariance structure from uncorrelated normal variables.
To remove the correlations between variables. This task requires using the inverse Cholesky transformation.
To quickly solve linear systems that involve a covariance matrix.

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

""" Outlier Detection Toolbox ========================= This is a single-file distribution (for ease of preview) of a production-grade outlier/anomaly detection toolbox intended to be split into a small package: outlier_detection/ ├── __init__.py ├── utils.py ├── statistical.py ├── distance_density.py ├── model_based.py ├── deep_learning.py ├── ensemble.py ├── visualization.py └── cli.py --- NOTE --- This code block contains *all* modules concatenated (with file headers) so you can preview and copy each file out into separate .py files. When you save them as separate files the package will work as expected. Design goals (what you asked for): - Detailed, well-documented functions (purpose, math, applicability, edge-cases) - Robust handling of NaNs, constant columns, categorical data - Functions return structured metadata + masks + scores so you can inspect - Utilities for ensemble combining methods and producing a readable report - Optional deep learning methods (AutoEncoder/VAE) with clear dependency instructions and graceful error messages if libraries are missing. Dependencies (recommended): pip install numpy pandas scipy scikit-learn matplotlib joblib tensorflow>=2.0 If you prefer PyTorch for deep models you can adapt deep_learning.py accordingly. """ # --------------------------- # File: outlier_detection/__init__.py # --------------------------- __version__ = "0.1.0" # make it easy to import core helpers from typing import Dict from .utils import ensure_dataframe, OutlierResult, summarize_results, recommend_methods from .statistical import z_score_method, modified_z_score, iqr_method, grubbs_test from .distance_density import lof_method, mahalanobis_method, dbscan_method, knn_distance_method from .model_based import ( isolation_forest_method, one_class_svm_method, pca_reconstruction_error, gmm_method, elliptic_envelope_method, ) # deep_learning module is optional (heavy dependency) try: from .deep_learning import autoencoder_method, vae_method except Exception: # graceful: user may not have TF installed; import will raise at use time autoencoder_method = None vae_method = None from .ensemble import ensemble_methods, aggregate_scores from .visualization import plot_boxplot, plot_pair_scatter __all__ = [ "__version__", "ensure_dataframe", "OutlierResult", "summarize_results", "recommend_methods", "z_score_method", "modified_z_score", "iqr_method", "grubbs_test", "lof_method", "mahalanobis_method", "dbscan_method", "knn_distance_method", "isolation_forest_method", "one_class_svm_method", "pca_reconstruction_error", "gmm_method", "elliptic_envelope_method", "autoencoder_method", "vae_method", "ensemble_methods", "aggregate_scores", "plot_boxplot", "plot_pair_scatter", ] # --------------------------- # File: outlier_detection/utils.py # --------------------------- """ Utilities for the outlier detection package. Key responsibilities: - Input validation and type normalization - Handling numeric / categorical separation - Standardization and robust scaling helpers - A consistent result object shape used by all detectors """ from typing import Dict, Any, Tuple, Optional, List import numpy as np import pandas as pd import logging logger = logging.getLogger(__name__) # A simple, documented result schema for detector functions. # Each detector returns a dict with these keys (guaranteed): # - 'mask': pd.Series[bool] same index as input rows; True means OUTLIER # - 'score': pd.Series or pd.DataFrame numeric score (bigger usually means more anomalous) # - 'method': short string # - 'params': dict of parameters used # - 'explanation': short textual note about interpretation OutlierResult = Dict[str, Any] def ensure_dataframe(X) -> pd.DataFrame: """ Convert input into a pandas DataFrame with a stable integer index. Accepts: pd.DataFrame, np.ndarray, list-of-lists, pd.Series. Returns DataFrame with numeric column names if necessary. """ if isinstance(X, pd.DataFrame): df = X.copy() elif isinstance(X, pd.Series): df = X.to_frame() else: # try to coerce df = pd.DataFrame(X) # if no index or non-unique, reset if df.index is None or not df.index.is_unique: df = df.reset_index(drop=True) # name numeric columns if unnamed df.columns = [str(c) for c in df.columns] return df def numeric_only(df: pd.DataFrame, return_cols: bool = False) -> pd.DataFrame: """ Select numeric columns and warn if non-numeric columns are dropped. If no numeric columns found raises ValueError. """ df = ensure_dataframe(df) numeric_df = df.select_dtypes(include=["number"]).copy() non_numeric = [c for c in df.columns if c not in numeric_df.columns] if non_numeric: logger.debug("Dropping non-numeric columns for numeric-only detectors: %s", non_numeric) if numeric_df.shape[1] == 0: raise ValueError("No numeric columns available for numeric detectors. Consider encoding categoricals.") if return_cols: return numeric_df, list(numeric_df.columns) return numeric_df def handle_missing(df: pd.DataFrame, strategy: str = "drop", fill_value: Optional[float] = None) -> pd.DataFrame: """ Handle missing values in data before passing to detectors. Parameters ---------- df : DataFrame strategy : {'drop', 'mean', 'median', 'zero', 'constant', 'keep'} - 'drop' : drop rows with any NaN (useful when most values are present) - 'mean' : fill numeric columns with mean - 'median' : fill numeric with median - 'zero' : fill with 0 - 'constant' : fill with supplied fill_value - 'keep' : keep NaNs (many detectors can handle NaN rows implicitly) fill_value : numeric (used when strategy=='constant') Returns ------- DataFrame cleaned according to strategy. Original index preserved. Notes ----- - Some detectors (LOF, IsolationForest) do NOT accept NaNs; choose strategy accordingly. """ df = df.copy() if strategy == "drop": return df.dropna(axis=0, how="any") elif strategy == "mean": return df.fillna(df.mean()) elif strategy == "median": return df.fillna(df.median()) elif strategy == "zero": return df.fillna(0) elif strategy == "constant": if fill_value is None: raise ValueError("fill_value must be provided for strategy='constant'") return df.fillna(fill_value) elif strategy == "keep": return df else: raise ValueError(f"Unknown missing value strategy: {strategy}") def robust_scale(df: pd.DataFrame) -> pd.DataFrame: """ Scale numeric columns using median and IQR (robust to outliers). Returns a DataFrame of same shape with scaled values. """ df = numeric_only(df) med = df.median() q1 = df.quantile(0.25) q3 = df.quantile(0.75) iqr = q3 - q1 # avoid division by zero iqr_replaced = iqr.replace(0, 1.0) return (df - med) / iqr_replaced def create_result(mask: pd.Series, score: pd.Series, method: str, params: Dict[str, Any], explanation: str) -> OutlierResult: """ Wrap mask + score into the standard result dict. """ # ensure index alignment if not mask.index.equals(score.index): # try to reindex score = score.reindex(mask.index) return { "mask": mask.astype(bool), "score": score, "method": method, "params": params, "explanation": explanation, } def summarize_results(results: Dict[str, OutlierResult]) -> pd.DataFrame: """ Given a dict of results keyed by method name, return a single DataFrame where each column is that method's boolean flag and another column is the score (if numeric). Also returns a short per-row summary like how many detectors flagged the row. """ # Collect masks and scores masks = {} scores = {} for k, r in results.items(): masks[f"{k}_flag"] = r["mask"].astype(int) # flatten score: if DataFrame use mean across columns sc = r["score"] if isinstance(sc, pd.DataFrame): sc = sc.mean(axis=1) scores[f"{k}_score"] = sc masks_df = pd.DataFrame(masks) scores_df = pd.DataFrame(scores) combined = pd.concat([masks_df, scores_df], axis=1) combined.index = next(iter(results.values()))["mask"].index combined["n_flags"] = masks_df.sum(axis=1) combined["any_flag"] = combined["n_flags"] > 0 return combined def recommend_methods(X: pd.DataFrame) -> List[str]: """ Heuristic recommender: returns a short list of methods to try depending on data shape. Rules (simple heuristics): - single numeric column: ['iqr', 'modified_z'] - low-dimensional (n_features <= 10) and numeric: ['mahalanobis','lof','isolation_forest'] - high-dimensional (n_features > 10): ['isolation_forest','pca','autoencoder'] """ df = ensure_dataframe(X) n_features = df.select_dtypes(include=["number"]).shape[1] if n_features == 0: raise ValueError("No numeric features to recommend methods for") if n_features == 1: return ["iqr", "modified_z"] elif n_features <= 10: return ["mahalanobis", "lof", "isolation_forest"] else: return ["isolation_forest", "pca", "autoencoder"] # --------------------------- # File: outlier_detection/statistical.py # --------------------------- """ Statistical / univariate outlier detectors. Each function focuses on single-dimension input (pd.Series) or will operate column-wise if given a DataFrame (then returns DataFrame of scores / masks). """ from typing import Union import numpy as np import pandas as pd from scipy import stats from .utils import create_result, numeric_only def _as_series(x: Union[pd.Series, pd.DataFrame], col: str = None) -> pd.Series: if isinstance(x, pd.DataFrame): if col is None: raise ValueError("If passing DataFrame, must pass column name") return x[col] return x def z_score_method(x: Union[pd.Series, pd.DataFrame], threshold: float = 3.0) -> OutlierResult: """ Z-Score method (univariate) Math: z = (x - mean) / std Flag where |z| > threshold. Applicability: single numeric column, approximately normal distribution. Not robust to heavy-tailed distributions. Returns OutlierResult with score = |z| (higher => more anomalous). """ if isinstance(x, pd.DataFrame): # apply per-column and return a DataFrame score masks = pd.DataFrame(index=x.index) scores = pd.DataFrame(index=x.index) for c in x.columns: res = z_score_method(x[c], threshold=threshold) masks[c] = res["mask"].astype(int) scores[c] = res["score"] # Derive a combined mask: any column flagged mask_any = masks.sum(axis=1) > 0 combined_score = scores.mean(axis=1) return create_result(mask_any, combined_score, "z_score_dataframe", {"threshold": threshold}, "Applied z-score per-column and combined by mean score and any-flag") s = x.dropna() if s.shape[0] == 0: mask = pd.Series([False]*len(x), index=x.index) score = pd.Series([0.0]*len(x), index=x.index) return create_result(mask, score, "z_score", {"threshold": threshold}, "Empty or all-NaN series") mu = s.mean() sigma = s.std(ddof=0) if sigma == 0: score = pd.Series(0.0, index=x.index) mask = pd.Series(False, index=x.index) explanation = "Zero variance: no z-score possible" return create_result(mask, score, "z_score", {"threshold": threshold}, explanation) z = (x - mu) / sigma absz = z.abs() mask = absz > threshold score = absz.fillna(0.0) explanation = f"z-score with mean={mu:.4g}, std={sigma:.4g}; flag |z|>{threshold}" return create_result(mask, score, "z_score", {"threshold": threshold}, explanation) def modified_z_score(x: Union[pd.Series, pd.DataFrame], threshold: float = 3.5) -> OutlierResult: """ Modified Z-score using median and MAD (robust to extreme values). Formula: M_i = 0.6745 * (x_i - median) / MAD Where MAD = median(|x_i - median|) Recommended threshold: 3.5 (common in literature) """ if isinstance(x, pd.DataFrame): masks = pd.DataFrame(index=x.index) scores = pd.DataFrame(index=x.index) for c in x.columns: res = modified_z_score(x[c], threshold=threshold) masks[c] = res["mask"].astype(int) scores[c] = res["score"] mask_any = masks.sum(axis=1) > 0 combined_score = scores.mean(axis=1) return create_result(mask_any, combined_score, "modified_z_dataframe", {"threshold": threshold}, "Applied modified z per-column and combined") s = x.dropna() if len(s) == 0: return create_result(pd.Series(False, index=x.index), pd.Series(0.0, index=x.index), "modified_z", {"threshold": threshold}, "empty") med = np.median(s) mad = np.median(np.abs(s - med)) if mad == 0: # all equal or too small score = pd.Series(0.0, index=x.index) mask = pd.Series(False, index=x.index) return create_result(mask, score, "modified_z", {"threshold": threshold}, "mad==0: no variation") M = 0.6745 * (x - med) / mad score = M.abs().fillna(0.0) mask = score > threshold return create_result(mask, score, "modified_z", {"threshold": threshold, "median": med, "mad": mad}, "Robust modified z-score; higher => more anomalous") def iqr_method(x: Union[pd.Series, pd.DataFrame], k: float = 1.5) -> OutlierResult: """ IQR (boxplot) method. Flags points outside [Q1 - k*IQR, Q3 + k*IQR]. k=1.5 is common; use larger k for fewer false positives. """ if isinstance(x, pd.DataFrame): masks = pd.DataFrame(index=x.index) scores = pd.DataFrame(index=x.index) for c in x.columns: res = iqr_method(x[c], k=k) masks[c] = res["mask"].astype(int) scores[c] = res["score"] mask_any = masks.sum(axis=1) > 0 combined_score = scores.mean(axis=1) return create_result(mask_any, combined_score, "iqr_dataframe", {"k": k}, "Applied IQR per column") s = x.dropna() if s.shape[0] == 0: return create_result(pd.Series(False, index=x.index), pd.Series(0.0, index=x.index), "iqr", {"k": k}, "empty") q1 = np.percentile(s, 25) q3 = np.percentile(s, 75) iqr = q3 - q1 lower = q1 - k * iqr upper = q3 + k * iqr mask = (x < lower) | (x > upper) # score: distance from nearest fence normalized by iqr (if iqr==0 use abs distance) if iqr == 0: score = (x - q1).abs().fillna(0.0) else: score = pd.Series(0.0, index=x.index) score[x < lower] = ((lower - x[x < lower]) / (iqr + 1e-12)) score[x > upper] = ((x[x > upper] - upper) / (iqr + 1e-12)) return create_result(mask.fillna(False), score.fillna(0.0), "iqr", {"k": k, "q1": q1, "q3": q3}, f"IQR fences [{lower:.4g}, {upper:.4g}]") def grubbs_test(x: Union[pd.Series, pd.DataFrame], alpha: float = 0.05) -> OutlierResult: """ Grubbs' test for a single outlier (requires approx normality). This test is intended to *detect one outlier at a time*. Use iteratively (recompute after removing detected outlier) if you expect multiple outliers, but be careful with multiplicity adjustments. Returns mask with at most one True (the most extreme point) unless alpha is very large. """ # For simplicity operate only on a single series. If DataFrame provided, # run per-column and combine (like other funcs) if isinstance(x, pd.DataFrame): masks = pd.DataFrame(index=x.index) scores = pd.DataFrame(index=x.index) for c in x.columns: res = grubbs_test(x[c], alpha=alpha) masks[c] = res["mask"].astype(int) scores[c] = res["score"] mask_any = masks.sum(axis=1) > 0 combined_score = scores.mean(axis=1) return create_result(mask_any, combined_score, "grubbs_dataframe", {"alpha": alpha}, "Applied Grubbs per column") from math import sqrt s = x.dropna() n = len(s) if n < 3: return create_result(pd.Series(False, index=x.index), pd.Series(0.0, index=x.index), "grubbs", {"alpha": alpha}, "n<3: cannot run") mean = s.mean() std = s.std(ddof=0) if std == 0: return create_result(pd.Series(False, index=x.index), pd.Series(0.0, index=x.index), "grubbs", {"alpha": alpha}, "zero std") # compute G statistic for max dev deviations = (s - mean).abs() max_idx = deviations.idxmax() G = deviations.loc[max_idx] / std # critical value from t-distribution t_crit = stats.t.ppf(1 - alpha / (2 * n), n - 2) G_crit = ((n - 1) / sqrt(n)) * (t_crit / sqrt(n - 2 + t_crit ** 2)) mask = pd.Series(False, index=x.index) mask.loc[max_idx] = G > G_crit score = pd.Series(0.0, index=x.index) score.loc[max_idx] = float(G) explanation = f"G={G:.4g}, Gcrit={G_crit:.4g}, alpha={alpha}" return create_result(mask, score, "grubbs", {"alpha": alpha, "G": G, "Gcrit": G_crit}, explanation) # --------------------------- # File: outlier_detection/distance_density.py # --------------------------- """ Distance and density based detectors (multivariate-capable). Functions generally accept a numeric DataFrame X and return OutlierResult. """ from sklearn.neighbors import LocalOutlierFactor, NearestNeighbors from sklearn.cluster import DBSCAN from sklearn.covariance import EmpiricalCovariance from .utils import ensure_dataframe, create_result, numeric_only def lof_method(X, n_neighbors: int = 20, contamination: float = 0.05) -> OutlierResult: """ Local Outlier Factor (LOF). Returns score = -lof. LOF API returns negative_outlier_factor_. We negate so higher score => more anomalous. Applicability: medium-dimensional data, clusters of varying density. Beware: LOF does not provide a predictable probabilistic threshold. """ X = ensure_dataframe(X) Xnum = numeric_only(X) if Xnum.shape[0] < 2: return create_result(pd.Series(False, index=X.index), pd.Series(0.0, index=X.index), "lof", {"n_neighbors": n_neighbors}, "too few samples") lof = LocalOutlierFactor(n_neighbors=min(n_neighbors, max(1, Xnum.shape[0]-1)), contamination=contamination) y = lof.fit_predict(Xnum) negative_factor = lof.negative_outlier_factor_ # higher -> more anomalous score = (-negative_factor) score = pd.Series(score, index=Xnum.index) mask = pd.Series(y == -1, index=Xnum.index) return create_result(mask, score, "lof", {"n_neighbors": n_neighbors, "contamination": contamination}, "LOF: higher score more anomalous") def knn_distance_method(X, k: int = 5) -> OutlierResult: """ k-NN distance based scoring: compute distance to k-th nearest neighbor. Points with large k-distance are candidate outliers. Returns score = k-distance (bigger => more anomalous). """ X = ensure_dataframe(X) Xnum = numeric_only(X) if Xnum.shape[0] < k + 1: return create_result(pd.Series(False, index=X.index), pd.Series(0.0, index=X.index), "knn_distance", {"k": k}, "too few samples") nbrs = NearestNeighbors(n_neighbors=k + 1).fit(Xnum) distances, _ = nbrs.kneighbors(Xnum) # distances[:, 0] is zero (self). take k-th neighbor kdist = distances[:, k] score = pd.Series(kdist, index=Xnum.index) # threshold: e.g., mean + 2*std thr = score.mean() + 2 * score.std() mask = score > thr return create_result(mask, score, "knn_distance", {"k": k, "threshold": thr}, "k-distance method") def mahalanobis_method(X, threshold_p: float = 0.01) -> OutlierResult: """ Mahalanobis distance based detection. Computes D^2 for each point. One can threshold by chi-square quantile with df=n_features: P(D^2 > thresh) = threshold_p. We return score = D^2. Applicability: data approximately elliptical (multivariate normal-ish). """ X = ensure_dataframe(X) Xnum = numeric_only(X) n, d = Xnum.shape if n <= d: # covariance ill-conditioned; apply shrinkage or PCA beforehand explanation = "n <= n_features: covariance may be singular, consider PCA or regularization" else: explanation = "" cov = EmpiricalCovariance().fit(Xnum) mahal = cov.mahalanobis(Xnum) score = pd.Series(mahal, index=Xnum.index) # default threshold: chi2 quantile from scipy.stats import chi2 thr = chi2.ppf(1 - threshold_p, df=d) if d > 0 else np.inf mask = score > thr return create_result(mask, score, "mahalanobis", {"threshold_p": threshold_p, "chi2_thr": float(thr)}, explanation) def dbscan_method(X, eps: float = 0.5, min_samples: int = 5) -> OutlierResult: """ DBSCAN clusterer: points labeled -1 are considered noise -> outliers. Applicability: non-spherical clusters, variable density; choose eps carefully. """ X = ensure_dataframe(X) Xnum = numeric_only(X) if Xnum.shape[0] < min_samples: return create_result(pd.Series(False, index=X.index), pd.Series(0.0, index=X.index), "dbscan", {"eps": eps, "min_samples": min_samples}, "too few samples") db = DBSCAN(eps=eps, min_samples=min_samples).fit(Xnum) labels = db.labels_ mask = pd.Series(labels == -1, index=Xnum.index) # score: negative of cluster size (noise points get score 1) # To keep simple: noise -> 1, else 0 score = pd.Series((labels == -1).astype(float), index=Xnum.index) return create_result(mask, score, "dbscan", {"eps": eps, "min_samples": min_samples}, "DBSCAN noise points flagged") # --------------------------- # File: outlier_detection/model_based.py # --------------------------- """ Model-based detectors: tree ensembles, SVM boundary, PCA reconstruction, GMM These functions are intended for multivariate numeric data. """ from sklearn.ensemble import IsolationForest from sklearn.svm import OneClassSVM from sklearn.decomposition import PCA from sklearn.mixture import GaussianMixture from sklearn.covariance import EllipticEnvelope from .utils import ensure_dataframe, numeric_only, create_result def isolation_forest_method(X, contamination: float = 0.05, random_state: int = 42) -> OutlierResult: """ Isolation Forest Returns mask and anomaly score (higher => more anomalous). Good general-purpose method for medium-to-high dimensional data. """ X = ensure_dataframe(X) Xnum = numeric_only(X) if Xnum.shape[0] < 2: return create_result(pd.Series(False, index=X.index), pd.Series(0.0, index=X.index), "isolation_forest", {"contamination": contamination}, "too few samples") iso = IsolationForest(contamination=contamination, random_state=random_state) iso.fit(Xnum) pred = iso.predict(Xnum) # decision_function: higher -> more normal, so we invert raw_score = -iso.decision_function(Xnum) score = pd.Series(raw_score, index=Xnum.index) mask = pd.Series(pred == -1, index=Xnum.index) return create_result(mask, score, "isolation_forest", {"contamination": contamination}, "IsolationForest: inverted decision function as score") def one_class_svm_method(X, kernel: str = "rbf", nu: float = 0.05, gamma: str = "scale") -> OutlierResult: """ One-Class SVM for boundary-based anomaly detection. Carefully tune nu and gamma; not robust to large datasets without subsampling. """ X = ensure_dataframe(X) Xnum = numeric_only(X) if Xnum.shape[0] < 5: return create_result(pd.Series(False, index=X.index), pd.Series(0.0, index=X.index), "one_class_svm", {"nu": nu}, "too few samples") ocsvm = OneClassSVM(kernel=kernel, nu=nu, gamma=gamma) ocsvm.fit(Xnum) pred = ocsvm.predict(Xnum) # decision_function: positive => inside boundary (normal); invert raw_score = -ocsvm.decision_function(Xnum) score = pd.Series(raw_score, index=Xnum.index) mask = pd.Series(pred == -1, index=Xnum.index) return create_result(mask, score, "one_class_svm", {"nu": nu, "kernel": kernel}, "OneClassSVM: invert decision_function for anomaly score") def pca_reconstruction_error(X, n_components: int = None, explained_variance: float = None, threshold: float = None) -> OutlierResult: """ PCA-based reconstruction error. If n_components not set, choose the minimum components to reach explained_variance (if provided). Otherwise uses min(n_features, 2). Score: squared reconstruction error per sample. Default threshold: mean+3*std. """ X = ensure_dataframe(X) Xnum = numeric_only(X) n, d = Xnum.shape if n == 0 or d == 0: return create_result(pd.Series(False, index=X.index), pd.Series(0.0, index=X.index), "pca_recon", {}, "empty data") if n_components is None: if explained_variance is not None: temp_pca = PCA(n_components=min(n, d)) temp_pca.fit(Xnum) cum = np.cumsum(temp_pca.explained_variance_ratio_) n_components = int(np.searchsorted(cum, explained_variance) + 1) n_components = max(1, n_components) else: n_components = min(2, d) pca = PCA(n_components=n_components) proj = pca.fit_transform(Xnum) recon = pca.inverse_transform(proj) errors = ((Xnum - recon) ** 2).sum(axis=1) score = pd.Series(errors, index=Xnum.index) if threshold is None: threshold = score.mean() + 3 * score.std() mask = score > threshold return create_result(mask, score, "pca_recon", {"n_components": n_components, "threshold": float(threshold)}, "PCA reconstruction error") def gmm_method(X, n_components: int = 2, contamination: float = 0.05) -> OutlierResult: """ Gaussian Mixture Model based anomaly score (log-likelihood). Score: negative log-likelihood (bigger => less likely => more anomalous). Threshold: empirical quantile of scores. """ X = ensure_dataframe(X) Xnum = numeric_only(X) if Xnum.shape[0] < n_components: return create_result(pd.Series(False, index=X.index), pd.Series(0.0, index=X.index), "gmm", {}, "too few samples") gmm = GaussianMixture(n_components=n_components) gmm.fit(Xnum) logprob = gmm.score_samples(Xnum) score = pd.Series(-logprob, index=Xnum.index) thr = score.quantile(1 - contamination) mask = score > thr return create_result(mask, score, {"n_components": n_components, "threshold": float(thr)}, "gmm", "GMM negative log-likelihood") def elliptic_envelope_method(X, contamination: float = 0.05) -> OutlierResult: """ EllipticEnvelope fits a robust covariance (assumes data come from a Gaussian-like ellipse). Flags outliers outside the ellipse. """ X = ensure_dataframe(X) Xnum = numeric_only(X) ee = EllipticEnvelope(contamination=contamination) ee.fit(Xnum) pred = ee.predict(Xnum) # decision_function: larger -> more normal; invert raw_score = -ee.decision_function(Xnum) score = pd.Series(raw_score, index=Xnum.index) mask = pd.Series(pred == -1, index=Xnum.index) return create_result(mask, score, "elliptic_envelope", {"contamination": contamination}, "EllipticEnvelope") # --------------------------- # File: outlier_detection/deep_learning.py # --------------------------- """ Deep learning based detectors (AutoEncoder, VAE). These require TensorFlow/Keras installed. If not present, importing this module will raise an informative ImportError. Design: a training function accepts X (numpy or DataFrame) and returns a callable `score_fn(X_new) -> pd.Series` plus a threshold selection helper. """ from typing import Callable import numpy as np import pandas as pd # lazy import to avoid hard TF dependency if user doesn't need it try: import tensorflow as tf from tensorflow.keras import layers, models, backend as K except Exception as e: raise ImportError("TensorFlow / Keras is required for deep_learning module. Install with `pip install tensorflow`. Error: " + str(e)) from .utils import ensure_dataframe, create_result def _build_autoencoder(input_dim: int, latent_dim: int = 8, hidden_units=(64, 32)) -> models.Model: inp = layers.Input(shape=(input_dim,)) x = inp for h in hidden_units: x = layers.Dense(h, activation='relu')(x) z = layers.Dense(latent_dim, activation='relu', name='latent')(x) x = z for h in reversed(hidden_units): x = layers.Dense(h, activation='relu')(x) out = layers.Dense(input_dim, activation='linear')(x) ae = models.Model(inp, out) return ae def autoencoder_method(X, latent_dim: int = 8, hidden_units=(128, 64), epochs: int = 50, batch_size: int = 32, validation_split: float = 0.1, threshold_method: str = 'quantile', threshold_val: float = 0.99, verbose: int = 0) -> OutlierResult: """ Train an AutoEncoder on X and compute reconstruction error as anomaly score. Parameters ---------- X : DataFrame or numpy array (numeric) threshold_method : 'quantile' or 'mean_std' threshold_val : if quantile -> e.g. 0.99 means top 1% flagged; if mean_std -> number of stds Returns ------- OutlierResult where score = reconstruction error and mask = score > threshold Notes ----- - This trains on the entire provided X. For actual anomaly detection, it's common to train the autoencoder only on "normal" data. If you have labels, pass only normal subset for training. - Requires careful scaling of inputs before training (robust_scale recommended). """ Xdf = ensure_dataframe(X) Xnum = Xdf.select_dtypes(include=['number']).fillna(0.0) input_dim = Xnum.shape[1] if input_dim == 0: return create_result(pd.Series(False, index=Xdf.index), pd.Series(0.0, index=Xdf.index), "autoencoder", {}, "no numeric columns") # convert to numpy arr = Xnum.values.astype(np.float32) ae = _build_autoencoder(input_dim=input_dim, latent_dim=latent_dim, hidden_units=hidden_units) ae.compile(optimizer='adam', loss='mse') ae.fit(arr, arr, epochs=epochs, batch_size=batch_size, validation_split=validation_split, verbose=verbose) recon = ae.predict(arr) errors = np.mean((arr - recon) ** 2, axis=1) score = pd.Series(errors, index=Xdf.index) if threshold_method == 'quantile': thr = float(score.quantile(threshold_val)) else: thr = float(score.mean() + threshold_val * score.std()) mask = score > thr return create_result(mask, score, "autoencoder", {"latent_dim": latent_dim, "threshold": thr}, "AutoEncoder reconstruction error") def vae_method(X, latent_dim: int = 8, hidden_units=(128, 64), epochs: int = 50, batch_size: int = 32, threshold_method: str = 'quantile', threshold_val: float = 0.99, verbose: int = 0) -> OutlierResult: """ Variational Autoencoder (VAE) anomaly detection. Implementation note: VAE is more involved; here we provide a simple implementation that uses reconstruction error as score. For strict probabilistic anomaly scoring one would use the ELBO / likelihood; this minimal implementation keeps it practical. """ # For brevity we reuse autoencoder path (a more complete VAE impl is possible) return autoencoder_method(X, latent_dim=latent_dim, hidden_units=hidden_units, epochs=epochs, batch_size=batch_size, threshold_method=threshold_method, threshold_val=threshold_val, verbose=verbose) # --------------------------- # File: outlier_detection/ensemble.py # --------------------------- """ Combine multiple detectors and produce an aggregated report. Provides strategies: union, intersection, majority voting, weighted sum of normalized scores. """ from typing import List, Dict import numpy as np import pandas as pd from .utils import ensure_dataframe, create_result def normalize_scores(scores: pd.DataFrame) -> pd.DataFrame: """Min-max normalize each score column to [0,1].""" sc = scores.copy() for c in sc.columns: col = sc[c] mn = col.min() mx = col.max() if mx == mn: sc[c] = 0.0 else: sc[c] = (col - mn) / (mx - mn) return sc def aggregate_scores(results: Dict[str, Dict], method: str = 'weighted', weights: Dict[str, float] = None) -> Dict: """ Aggregate multiple OutlierResult dictionaries produced by detectors. Returns an OutlierResult-like dict with: - mask (final boolean by threshold on aggregate score), - score (aggregate numeric score) Aggregation methods: - 'union' : any detector flagged => outlier (score = max of normalized scores) - 'intersection' : flagged by all detectors => outlier - 'majority' : flagged by >50% detectors - 'weighted' : weighted sum of normalized scores (weights provided or equal) """ # collect masks and scores into DataFrames masks = pd.DataFrame({k: v['mask'].astype(int) for k, v in results.items()}) raw_scores = pd.DataFrame({k: (v['score'] if isinstance(v['score'], pd.Series) else pd.Series(v['score'])) for k, v in results.items()}) raw_scores.index = masks.index norm_scores = normalize_scores(raw_scores) if method == 'union': agg_score = norm_scores.max(axis=1) elif method == 'intersection': agg_score = norm_scores.min(axis=1) elif method == 'majority': agg_score = masks.sum(axis=1) / max(1, masks.shape[1]) elif method == 'weighted': if weights is None: weights = {k: 1.0 for k in results.keys()} # align weights w = pd.Series({k: weights.get(k, 1.0) for k in results.keys()}) # make sure weights sum to 1 w = w / w.sum() agg_score = (norm_scores * w).sum(axis=1) else: raise ValueError("Unknown aggregation method") # default threshold: 0.5 mask = agg_score > 0.5 return create_result(mask, agg_score, f"ensemble_{method}", {"method": method}, "Aggregated ensemble score") def ensemble_methods(X, method_list: List[str] = None, method_params: Dict = None) -> Dict[str, Dict]: """ Convenience: run multiple detectors by name and return dict of results. method_list: list of names from ['iqr','modified_z','z_score','lof','mahalanobis','isolation_forest', ...] method_params: optional dict mapping method name to params """ from . import statistical, distance_density, model_based, deep_learning X = ensure_dataframe(X) if method_list is None: method_list = ['iqr', 'modified_z', 'isolation_forest', 'lof'] if method_params is None: method_params = {} results = {} for m in method_list: params = method_params.get(m, {}) try: if m == 'iqr': results[m] = statistical.iqr_method(X, **params) elif m == 'modified_z': results[m] = statistical.modified_z_score(X, **params) elif m == 'z_score': results[m] = statistical.z_score_method(X, **params) elif m == 'lof': results[m] = distance_density.lof_method(X, **params) elif m == 'mahalanobis': results[m] = distance_density.mahalanobis_method(X, **params) elif m == 'dbscan': results[m] = distance_density.dbscan_method(X, **params) elif m == 'knn': results[m] = distance_density.knn_distance_method(X, **params) elif m == 'isolation_forest': results[m] = model_based.isolation_forest_method(X, **params) elif m == 'one_class_svm': results[m] = model_based.one_class_svm_method(X, **params) elif m == 'pca': results[m] = model_based.pca_reconstruction_error(X, **params) elif m == 'gmm': results[m] = model_based.gmm_method(X, **params) elif m == 'elliptic': results[m] = model_based.elliptic_envelope_method(X, **params) elif m == 'autoencoder': results[m] = deep_learning.autoencoder_method(X, **params) else: logger.warning("Unknown method requested: %s", m) except Exception as e: logger.exception("Method %s failed: %s", m, e) return results # --------------------------- # File: outlier_detection/visualization.py # --------------------------- """ Simple plotting helpers for quick inspection. Note: plotting is intentionally minimal; for report-quality figures users can adapt styles. The functions return the matplotlib Figure object so they can be further customized. """ import matplotlib.pyplot as plt from .utils import ensure_dataframe def plot_boxplot(series: pd.Series, show: bool = True): df = ensure_dataframe(series) col = df.columns[0] fig, ax = plt.subplots() ax.boxplot(df[col].dropna()) ax.set_title(f"Boxplot: {col}") if show: plt.show() return fig def plot_pair_scatter(X, columns: list = None, show: bool = True): X = ensure_dataframe(X) if columns is not None: X = X[columns] cols = X.columns.tolist()[:4] # avoid huge plots fig, axes = plt.subplots(len(cols) - 1, len(cols) - 1, figsize=(4 * (len(cols) - 1), 4 * (len(cols) - 1))) for i in range(1, len(cols)): for j in range(i): ax = axes[i - 1, j] ax.scatter(X[cols[j]], X[cols[i]], s=8) ax.set_xlabel(cols[j]) ax.set_ylabel(cols[i]) fig.suptitle("Pairwise scatter (first 4 numeric cols)") if show: plt.show() return fig # --------------------------- # File: outlier_detection/cli.py # --------------------------- """ A very small CLI to run detectors on a CSV file and output a CSV report. Usage (example): python -m outlier_detection.cli detect input.csv output_report.csv --methods iqr,isolation_forest """ import argparse import pandas as pd from .ensemble import ensemble_methods, aggregate_scores def main(): parser = argparse.ArgumentParser(description='Outlier detection CLI') sub = parser.add_subparsers(dest='cmd') det = sub.add_parser('detect') det.add_argument('input_csv') det.add_argument('output_csv') det.add_argument('--methods', default='iqr,modified_z,isolation_forest,lof') args = parser.parse_args() df = pd.read_csv(args.input_csv) methods = args.methods.split(',') results = ensemble_methods(df, method_list=methods) agg = aggregate_scores(results, method='weighted') summary = pd.concat([pd.DataFrame({k: v['mask'].astype(int) for k, v in results.items()}), pd.DataFrame({k: v['score'] for k, v in results.items()})], axis=1) summary['ensemble_score'] = agg['score'] summary['ensemble_flag'] = agg['mask'].astype(int) summary.to_csv(args.output_csv, index=False) print(f"Wrote report to {args.output_csv}") if __name__ == '__main__': main()改成中文说明并返回代码给我
最新发布
08-27
/* * Copyright 2018-2019 Autoware Foundation. All rights reserved. * * Licensed under 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. */ #include "ekf_localizer/ekf_localizer.h" // clang-format off #define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl #define DEBUG_INFO(...) { if (show_debug_info_) { ROS_INFO(__VA_ARGS__); } } #define DEBUG_PRINT_MAT(X) { if (show_debug_info_) { std::cout << #X << ": " << X << std::endl; } } // clang-format on /* x, y:机器人位置。 yaw:机器人朝向(偏航角)。 yaw_bias:偏航角偏差(用于估计传感器误差)。 vx, wz:线速度和角速度。 */ EKFLocalizer::EKFLocalizer() : nh_(""), pnh_("~"), dim_x_(8 /* x, y, yaw, yaw_bias, vx, wz */) { pnh_.param("show_debug_info", show_debug_info_, bool(false)); // 是否显示调试信息 pnh_.param("predict_frequency", ekf_rate_, double(50.0)); // EKF 预测频率(Hz) ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); // 计算时间步长(秒) pnh_.param("enable_yaw_bias_estimation", enable_yaw_bias_estimation_, bool(true)); // 是否估计偏航角偏差 pnh_.param("extend_state_step", extend_state_step_, int(50)); // 状态扩展步数(用于滑动窗口优化) pnh_.param("pose_frame_id", pose_frame_id_, std::string("map")); // 输出位姿的坐标系 ID pnh_.param("output_frame_id", output_frame_id_, std::string("base_link")); // 输出坐标系 /* pose measurement 位姿测量参数*/ pnh_.param("pose_additional_delay", pose_additional_delay_, double(0.0)); // 额外延迟(秒) pnh_.param("pose_measure_uncertainty_time", pose_measure_uncertainty_time_, double(0.01)); // 测量不确定性时间 pnh_.param("pose_rate", pose_rate_, double(10.0)); // 位姿测量频率(用于协方差计算) // used for covariance calculation pnh_.param("pose_gate_dist", pose_gate_dist_, double(10000.0)); // 马氏距离阈值(异常值过滤) // Mahalanobis limit pnh_.param("pose_stddev_x", pose_stddev_x_, double(0.05)); // X 方向标准差(米) pnh_.param("pose_stddev_y", pose_stddev_y_, double(0.05)); // Y 方向标准差(米) pnh_.param("pose_stddev_yaw", pose_stddev_yaw_, double(0.035)); // 偏航角标准差(弧度) pnh_.param("use_pose_with_covariance", use_pose_with_covariance_, bool(false)); // 是否使用带协方差的位姿输入 /* twist measurement 速度测量参数*/ pnh_.param("twist_additional_delay", twist_additional_delay_, double(0.0)); // 额外延迟(秒) pnh_.param("twist_rate", twist_rate_, double(10.0)); // 速度测量频率(用于协方差计算) // used for covariance calculation pnh_.param("twist_gate_dist", twist_gate_dist_, double(10000.0)); // 马氏距离阈值(异常值过滤) // Mahalanobis limit pnh_.param("twist_stddev_vx", twist_stddev_vx_, double(0.2)); // 线速度标准差(米/秒) pnh_.param("twist_stddev_wz", twist_stddev_wz_, double(0.03)); // 角速度标准差(弧度/秒) pnh_.param("use_twist_with_covariance", use_twist_with_covariance_, bool(false)); // 是否使用带协方差的速度输入 /* IMU measurement parameters */ pnh_.param("use_imu", use_imu_, bool(true)); pnh_.param("imu_rate", imu_rate_, double(50.0)); pnh_.param("imu_gate_dist", imu_gate_dist_, double(10000.0)); pnh_.param("imu_stddev_ax", imu_stddev_ax_, double(0.5)); pnh_.param("imu_stddev_wz", imu_stddev_wz_, double(0.01)); /* process noise 过程噪声参数*/ double proc_stddev_yaw_c, proc_stddev_yaw_bias_c, proc_stddev_vx_c, proc_stddev_wz_c; double proc_stddev_ax_c, proc_stddev_wz_imu_c; pnh_.param("proc_stddev_yaw_c", proc_stddev_yaw_c, double(0.005)); // 偏航角过程噪声(连续时间) pnh_.param("proc_stddev_yaw_bias_c", proc_stddev_yaw_bias_c, double(0.001)); // 偏航角偏差过程噪声 pnh_.param("proc_stddev_vx_c", proc_stddev_vx_c, double(2.0)); // 线速度过程噪声 pnh_.param("proc_stddev_wz_c", proc_stddev_wz_c, double(0.2)); // 角速度过程噪声 if (!enable_yaw_bias_estimation_) { proc_stddev_yaw_bias_c = 0.0; } /* convert to continuous to discrete 转换为离散时间噪声(乘以时间步长)*/ proc_cov_vx_d_ = std::pow(proc_stddev_vx_c, 2.0) * ekf_dt_; proc_cov_wz_d_ = std::pow(proc_stddev_wz_c, 2.0) * ekf_dt_; proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c, 2.0) * ekf_dt_; proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c, 2.0) * ekf_dt_; proc_cov_ax_d_ = std::pow(proc_stddev_ax_c, 2.0) * ekf_dt_; proc_cov_wz_imu_d_ = std::pow(proc_stddev_wz_imu_c, 2.0) * ekf_dt_; /* initialize ros system */ // 定时器(用于 EKF 预测步) timer_control_ = nh_.createTimer(ros::Duration(ekf_dt_), &EKFLocalizer::timerCallback, this); // 发布话题 //pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("/ekf_pose", 1); pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 10); pub_pose_cov_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("ekf_pose_with_covariance", 10); //pub_twist_ = nh_.advertise<geometry_msgs::TwistStamped>("/ekf_twist", 1); pub_twist_ = nh_.advertise<geometry_msgs::TwistStamped>("/estimate_twist", 10); pub_twist_cov_ = nh_.advertise<geometry_msgs::TwistWithCovarianceStamped>("ekf_twist_with_covariance", 10); pub_yaw_bias_ = pnh_.advertise<std_msgs::Float64>("estimated_yaw_bias", 10); // 订阅话题 sub_initialpose_ = nh_.subscribe("initialpose", 10, &EKFLocalizer::callbackInitialPose, this); sub_pose_with_cov_ = nh_.subscribe("in_pose_with_covariance", 10, &EKFLocalizer::callbackPoseWithCovariance, this); sub_pose_ = nh_.subscribe("/in_pose", 10, &EKFLocalizer::callbackPose, this); sub_twist_with_cov_ = nh_.subscribe("in_twist_with_covariance", 10, &EKFLocalizer::callbackTwistWithCovariance, this); //sub_twist_ = nh_.subscribe("/can_info", 10, &EKFLocalizer::callbackTwist, this); imu_sub_.subscribe(nh_, "/imu_raw", 100); vehicle_sub_.subscribe(nh_, "/can_info", 50); sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10)); sync_->connectInput(imu_sub_, vehicle_sub_); sync_->registerCallback(boost::bind(&EKFLocalizer::sensorCallback, this, _1, _2)); sync_->setMaxIntervalDuration(ros::Duration(0.003)); // 3ms容差 dim_x_ex_ = dim_x_ * extend_state_step_; // 扩展状态维度(用于滑动窗口优化) initEKF(); // 初始化 EKF 内部状态 last_timer_call_time_ = 0.0; /* debug */ pub_debug_ = pnh_.advertise<std_msgs::Float64MultiArray>("debug", 1); // 调试信息(数组) pub_measured_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("debug/measured_pose", 1); // 调试用测量位姿 pub_measured_imu_ = pnh_.advertise<sensor_msgs::Imu>("debug/measured_imu", 1); }; EKFLocalizer::~EKFLocalizer(){}; /* * timerCallback */ void EKFLocalizer::timerCallback(const ros::TimerEvent& e) { DEBUG_INFO("========================= timer called ========================="); /* predict model in EKF 预测步(Predict)*/ auto start = std::chrono::system_clock::now(); DEBUG_INFO("------------------------- start prediction -------------------------"); double actual_dt = (last_timer_call_time_ > 0.0) ? (ros::Time::now().toSec() - last_timer_call_time_) : ekf_dt_; predictKinematicsModel(actual_dt); // 执行运动模型预测 double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); // 计算耗时 DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6); DEBUG_INFO("------------------------- end prediction -------------------------\n"); /* pose measurement update */ if (current_pose_ptr_ != nullptr) // 位姿更新(当有新位姿数据时) { DEBUG_INFO("------------------------- start Pose -------------------------"); start = std::chrono::system_clock::now(); measurementUpdatePose(*current_pose_ptr_); // 融合传感器位姿数据 elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6); DEBUG_INFO("------------------------- end Pose -------------------------\n"); } /* twist measurement update */ if (current_twist_ptr_ != nullptr) // 速度更新(当有新速度数据时) { DEBUG_INFO("------------------------- start twist -------------------------"); start = std::chrono::system_clock::now(); measurementUpdateTwist(*current_twist_ptr_); // 融合速度数据 elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6); DEBUG_INFO("------------------------- end twist -------------------------\n"); } /* IMU measurement update */ if (use_imu_ && current_imu_ptr_ != nullptr) { DEBUG_INFO("------------------------- start IMU -------------------------"); start = std::chrono::system_clock::now(); measurementUpdateIMU(*current_imu_ptr_); elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); DEBUG_INFO("[EKF] measurementUpdateIMU calculation time = %f [ms]", elapsed * 1.0e-6); DEBUG_INFO("------------------------- end IMU -------------------------\n"); } /* set current pose, twist */ setCurrentResult(); // 更新内部状态 last_timer_call_time_ = ros::Time::now().toSec(); /* publish ekf result */ publishEstimateResult(); // 发布最终估计结果 } void EKFLocalizer::showCurrentX() { // 检查调试信息显示标志是否开启 if (show_debug_info_) { // 创建临时矩阵存储状态向量 Eigen::MatrixXd X(dim_x_, 1); // 从EKF获取最新状态估计值 ekf_.getLatestX(X); // 打印转置后的状态向量(行向量形式) DEBUG_PRINT_MAT(X.transpose()); } } /* * setCurrentResult */ void EKFLocalizer::setCurrentResult() { current_ekf_pose_.header.frame_id = pose_frame_id_; current_ekf_pose_.header.stamp = ros::Time::now(); current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X); current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y); tf2::Quaternion q_tf; double roll, pitch, yaw; if (current_pose_ptr_ != nullptr) { current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z; tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */ tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw); } else { current_ekf_pose_.pose.position.z = 0.0; roll = 0; pitch = 0; } yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB); q_tf.setRPY(roll, pitch, yaw); tf2::convert(q_tf, current_ekf_pose_.pose.orientation); current_ekf_twist_.header.frame_id = output_frame_id_; current_ekf_twist_.header.stamp = ros::Time::now(); current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX); current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ) + ekf_.getXelement(IDX::WZ_IMU); } /* * broadcastTF */ void EKFLocalizer::broadcastTF(ros::Time time) { // if (current_ekf_pose_.header.frame_id == "") // { // return; // } // tf::Transform transform; // transform.setOrigin(tf::Vector3(current_ekf_pose_.pose.position.x, current_ekf_pose_.pose.position.y, current_ekf_pose_.pose.position.z)); // tf::Quaternion current_q( // current_ekf_pose_.pose.orientation.x, // current_ekf_pose_.pose.orientation.y, // current_ekf_pose_.pose.orientation.z, // current_ekf_pose_.pose.orientation.w // ); // transform.setRotation(current_q); // tf_br_.sendTransform(tf::StampedTransform(transform, time, "/map", output_frame_id_)); if (current_ekf_pose_.header.frame_id == "") { return; } geometry_msgs::TransformStamped transformStamped; transformStamped.header = current_ekf_pose_.header; transformStamped.child_frame_id = output_frame_id_; transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x; transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y; transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z; transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x; transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y; transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z; transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w; tf_br_.sendTransform(transformStamped); } /* * getTransformFromTF */ bool EKFLocalizer::getTransformFromTF(std::string parent_frame, std::string child_frame, geometry_msgs::TransformStamped& transform) { // tf::TransformListener listener; // for (int i = 0; i < 50; ++i) // { // try // { // auto now = ros::Time(0); // listener.waitForTransform(parent_frame, child_frame, now, ros::Duration(10.0)); // listener.lookupTransform(parent_frame, child_frame, now, transform); // return true; // } // catch (tf::TransformException& ex) // { // ROS_ERROR("%s", ex.what()); // ros::Duration(0.1).sleep(); // } // } // return false; tf2_ros::Buffer tf_buffer; tf2_ros::TransformListener tf_listener(tf_buffer); ros::Duration(0.1).sleep(); if (parent_frame.front() == '/') parent_frame.erase(0, 1); if (child_frame.front() == '/') child_frame.erase(0, 1); for (int i = 0; i < 50; ++i) { try { transform = tf_buffer.lookupTransform(parent_frame, child_frame, ros::Time(0)); return true; } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); ros::Duration(0.1).sleep(); } } return false; } /* * callbackInitialPose */ void EKFLocalizer::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped& initialpose) { // (1) 获取 TF 变换 // tf::StampedTransform transform; // if (!getTransformFromTF(pose_frame_id_, initialpose.header.frame_id, transform)) // { // ROS_ERROR("[EKF] TF transform failed. parent = %s, child = %s", // pose_frame_id_.c_str(), initialpose.header.frame_id.c_str()); // return; // 必须返回,避免使用无效变换 // } // // (2) 初始化状态向量 X // Eigen::MatrixXd X(dim_x_, 1); // X.setZero(); // 显式初始化所有状态为 0 // // 将 initialpose 变换到 pose_frame_id_ 坐标系 // tf::Pose tf_initial_pose; // tf::poseMsgToTF(initialpose.pose.pose, tf_initial_pose); // tf::Pose transformed_pose = transform * tf_initial_pose; // 正确应用 TF 变换 // X(IDX::X) = transformed_pose.getOrigin().x(); // X(IDX::Y) = transformed_pose.getOrigin().y(); // X(IDX::YAW) = tf::getYaw(transformed_pose.getRotation()); // X(IDX::YAWB) = 0.0; // 偏航角偏差初始化为 0 // X(IDX::VX) = 0.0; // 速度初始化为 0 // X(IDX::WZ) = 0.0; // 角速度初始化为 0 // X(IDX::AX) = 0.0; // 加速度初始化为 0 // X(IDX::WZ_IMU) = 0.0; // IMU 角速度初始化为 0 // // (3) 初始化协方差矩阵 P // Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); // const auto& cov = initialpose.pose.covariance; // // 检查协方差矩阵是否有效(非负且非全零) // if (cov[0] > 0.0) P(IDX::X, IDX::X) = cov[0]; // X variance // if (cov[7] > 0.0) P(IDX::Y, IDX::Y) = cov[7]; // Y variance // if (cov[35] > 0.0) P(IDX::YAW, IDX::YAW) = cov[35]; // YAW variance // // 其他状态的协方差(默认值) // P(IDX::YAWB, IDX::YAWB) = 0.0001; // 偏航角偏差 // P(IDX::VX, IDX::VX) = 0.01; // 速度 // P(IDX::WZ, IDX::WZ) = 0.01; // 角速度 // P(IDX::AX, IDX::AX) = 0.01; // 加速度 // P(IDX::WZ_IMU, IDX::WZ_IMU) = 0.01; // IMU 角速度 // // (4) 初始化 EKF // ekf_.init(X, P, extend_state_step_); geometry_msgs::TransformStamped transform; if (!getTransformFromTF(pose_frame_id_, initialpose.header.frame_id, transform)) { ROS_ERROR("[EKF] TF transform failed. parent = %s, child = %s", pose_frame_id_.c_str(), initialpose.header.frame_id.c_str()); }; Eigen::MatrixXd X(dim_x_, 1); Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); X(IDX::X) = initialpose.pose.pose.position.x /* + transform.transform.translation.x */; X(IDX::Y) = initialpose.pose.pose.position.y /* + transform.transform.translation.y */; X(IDX::YAW) = tf2::getYaw(initialpose.pose.pose.orientation) /* + tf2::getYaw(transform.transform.rotation) */; X(IDX::YAWB) = 0.0; X(IDX::VX) = 0.0; X(IDX::WZ) = 0.0; X(IDX::AX) = 0.0; // 加速度初始化为 0 X(IDX::WZ_IMU) = 0.0; // IMU 角速度初始化为 0 P(IDX::X, IDX::X) = initialpose.pose.covariance[0]; P(IDX::Y, IDX::Y) = initialpose.pose.covariance[6 + 1]; P(IDX::YAW, IDX::YAW) = initialpose.pose.covariance[6 * 5 + 5]; P(IDX::YAWB, IDX::YAWB) = 0.0001; P(IDX::VX, IDX::VX) = 0.01; P(IDX::WZ, IDX::WZ) = 0.01; P(IDX::AX, IDX::AX) = 0.01; // 加速度 P(IDX::WZ_IMU, IDX::WZ_IMU) = 0.01; // IMU 角速度 ekf_.init(X, P, extend_state_step_); }; /* * callbackPose */ void EKFLocalizer::callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg) { if (!use_pose_with_covariance_) { current_pose_ptr_ = std::make_shared<geometry_msgs::PoseStamped>(*msg); } }; /* * callbackPoseWithCovariance */ void EKFLocalizer::callbackPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) { if (use_pose_with_covariance_) { geometry_msgs::PoseStamped pose; pose.header = msg->header; pose.pose = msg->pose.pose; current_pose_ptr_ = std::make_shared<geometry_msgs::PoseStamped>(pose); current_pose_covariance_ = msg->pose.covariance; } }; /* * callbackTwist */ void EKFLocalizer::sensorCallback(const sensor_msgs::Imu::ConstPtr& imu_msg, const autoware_can_msgs::CANInfo::ConstPtr& vehicle_msg) { geometry_msgs::TwistStamped twist_msg; twist_msg.header = vehicle_msg->header; twist_msg.header.frame_id = "/base_link"; // 根据实际坐标系设置 // 设置线速度 (来自CAN) twist_msg.twist.linear.x = (vehicle_msg->speed / 3.6) * cos(vehicle_msg->angle); twist_msg.twist.linear.y = 0.0; twist_msg.twist.linear.z = 0.0; // 设置角速度 (来自IMU) //twist_msg.twist.angular = imu_msg->angular_velocity; // 计算运动学模型的角速度 twist_msg.twist.angular.x = 0; twist_msg.twist.angular.y = 0; twist_msg.twist.angular.z = ((vehicle_msg->speed / 3.6)*sin(vehicle_msg->angle))/1.137; current_twist_ptr_ = std::make_shared<geometry_msgs::TwistStamped>(twist_msg); if (imu_msg->header.frame_id != "imu") { ROS_WARN_DELAYED_THROTTLE(2, "IMU frame_id is %s, but expected 'imu'", imu_msg->header.frame_id.c_str()); } current_imu_ptr_ = std::make_shared<sensor_msgs::Imu>(*imu_msg); // // 估计IMU零偏 // estimateIMUBias(); } /* * callbackTwistWithCovariance */ void EKFLocalizer::callbackTwistWithCovariance(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg) { if (use_twist_with_covariance_) { geometry_msgs::TwistStamped twist; twist.header = msg->header; twist.twist = msg->twist.twist; current_twist_ptr_ = std::make_shared<geometry_msgs::TwistStamped>(twist); current_twist_covariance_ = msg->twist.covariance; } }; /* * initEKF */ void EKFLocalizer::initEKF() { Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y P(IDX::YAW, IDX::YAW) = 50.0; // for yaw P(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias P(IDX::VX, IDX::VX) = 1000.0; // for vx P(IDX::WZ, IDX::WZ) = 50.0; // for wz P(IDX::AX, IDX::AX) = 10.0; P(IDX::WZ_IMU, IDX::WZ_IMU) = 1.0; ekf_.init(X, P, extend_state_step_); } /* * predictKinematicsModel */ void EKFLocalizer::predictKinematicsModel(double actual_dt) { Eigen::MatrixXd X_curr(dim_x_, 1); Eigen::MatrixXd X_next(dim_x_, 1); ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); Eigen::MatrixXd P_curr; ekf_.getLatestP(P_curr); const double yaw = X_curr(IDX::YAW); const double yaw_bias = X_curr(IDX::YAWB); const double vx = X_curr(IDX::VX); const double wz = X_curr(IDX::WZ); const double ax = X_curr(IDX::AX); const double wz_imu = X_curr(IDX::WZ_IMU); const double dt = actual_dt; /* Update for latest state */ X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt; X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt + 0.5 * ax * sin(yaw + yaw_bias) * dt * dt; X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz + wz_imu) * dt; X_next(IDX::YAWB) = yaw_bias; X_next(IDX::VX) = vx + ax * dt; X_next(IDX::WZ) = wz; X_next(IDX::AX) = ax; X_next(IDX::WZ_IMU) = wz_imu; X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW))); /* Set A matrix for latest state */ Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_); A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt - 0.5 * ax * sin(yaw + yaw_bias) * dt * dt; A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt - 0.5 * ax * sin(yaw + yaw_bias) * dt * dt; A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; A(IDX::X, IDX::AX) = 0.5 * cos(yaw + yaw_bias) * dt * dt; A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt; A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt; A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; A(IDX::Y, IDX::AX) = 0.5 * sin(yaw + yaw_bias) * dt * dt; A(IDX::YAW, IDX::WZ) = dt; A(IDX::YAW, IDX::WZ_IMU) = dt; A(IDX::VX, IDX::AX) = dt; /* Process noise covariance matrix Q */ Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_); // 位置过程噪声(由速度和加速度引起) const double dvx = std::sqrt(P_curr(IDX::VX, IDX::VX)); const double dax = std::sqrt(P_curr(IDX::AX, IDX::AX)); const double dyaw = std::sqrt(P_curr(IDX::YAW, IDX::YAW)); if (dvx < 10.0 && dyaw < 1.0 && dax < 5.0) { Eigen::MatrixXd Jp_pos = Eigen::MatrixXd::Zero(2, 3); Jp_pos << cos(yaw), -vx*sin(yaw), 0.5*cos(yaw), sin(yaw), vx*cos(yaw), 0.5*sin(yaw); Eigen::MatrixXd Q_vx_yaw_ax = Eigen::MatrixXd::Zero(3, 3); Q_vx_yaw_ax(0, 0) = P_curr(IDX::VX, IDX::VX) * dt; Q_vx_yaw_ax(1, 1) = P_curr(IDX::YAW, IDX::YAW) * dt; Q_vx_yaw_ax(2, 2) = P_curr(IDX::AX, IDX::AX) * dt; Q_vx_yaw_ax(0, 1) = P_curr(IDX::VX, IDX::YAW) * dt; Q_vx_yaw_ax(1, 0) = P_curr(IDX::YAW, IDX::VX) * dt; Q_vx_yaw_ax(0, 2) = P_curr(IDX::VX, IDX::AX) * dt; Q_vx_yaw_ax(2, 0) = P_curr(IDX::AX, IDX::VX) * dt; Q_vx_yaw_ax(1, 2) = P_curr(IDX::YAW, IDX::AX) * dt; Q_vx_yaw_ax(2, 1) = P_curr(IDX::AX, IDX::YAW) * dt; Q.block(0, 0, 2, 2) = Jp_pos * Q_vx_yaw_ax * Jp_pos.transpose(); } else { Q(IDX::X, IDX::X) = 0.1; Q(IDX::Y, IDX::Y) = 0.1; } // 角度过程噪声 Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_; Q(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // 速度过程噪声 Q(IDX::VX, IDX::VX) = proc_cov_vx_d_; Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_; // 加速度和IMU角速度过程噪声 Q(IDX::AX, IDX::AX) = proc_cov_ax_d_; Q(IDX::WZ_IMU, IDX::WZ_IMU) = proc_cov_wz_imu_d_; ekf_.predictWithDelay(X_next, A, Q); // debug Eigen::MatrixXd X_result(dim_x_, 1); ekf_.getLatestX(X_result); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); } /* * measurementUpdatePose */ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::PoseStamped& pose) { if (pose.header.frame_id != pose_frame_id_) { ROS_WARN_DELAYED_THROTTLE(2, "pose frame_id is %s, but pose_frame is set as %s. They must be same.", pose.header.frame_id.c_str(), pose_frame_id_.c_str()); } Eigen::MatrixXd X_curr(dim_x_, 1); // curent state ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output const ros::Time t_curr = ros::Time::now(); /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).toSec() + pose_additional_delay_; if (delay_time < 0.0) { delay_time = 0.0; ROS_WARN_DELAYED_THROTTLE(1.0, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { ROS_WARN_DELAYED_THROTTLE(1.0, "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); return; } DEBUG_INFO("delay_time: %f [s]", delay_time); /* Set yaw */ const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW)); double yaw = tf2::getYaw(pose.pose.orientation); const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW); const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi yaw = yaw_error + ekf_yaw; /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << pose.pose.position.x, pose.pose.position.y, yaw; if (isnan(y.array()).any() || isinf(y.array()).any()) { ROS_WARN("[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); return; } /* Gate */ Eigen::MatrixXd y_ekf(dim_y, 1); y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw; Eigen::MatrixXd P_curr, P_y; ekf_.getLatestP(P_curr); P_y = P_curr.block(0, 0, dim_y, dim_y); if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) { ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; } DEBUG_PRINT_MAT(y.transpose()); DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); /* Set measurement matrix */ Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw /* Set measurement noise covariancs */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (use_pose_with_covariance_) { R(0, 0) = current_pose_covariance_.at(0); // x - x R(0, 1) = current_pose_covariance_.at(1); // x - y R(0, 2) = current_pose_covariance_.at(5); // x - yaw R(1, 0) = current_pose_covariance_.at(6); // y - x R(1, 1) = current_pose_covariance_.at(7); // y - y R(1, 2) = current_pose_covariance_.at(11); // y - yaw R(2, 0) = current_pose_covariance_.at(30); // yaw - x R(2, 1) = current_pose_covariance_.at(31); // yaw - y R(2, 2) = current_pose_covariance_.at(35); // yaw - yaw } else { const double ekf_yaw = ekf_.getXelement(IDX::YAW); const double vx = ekf_.getXelement(IDX::VX); const double wz = ekf_.getXelement(IDX::WZ); const double cov_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0); const double cov_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0); const double cov_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0); R(0, 0) = std::pow(pose_stddev_x_, 2) + cov_pos_x; // pos_x R(1, 1) = std::pow(pose_stddev_y_, 2) + cov_pos_y; // pos_y R(2, 2) = std::pow(pose_stddev_yaw_, 2) + cov_yaw; // yaw } /* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every * step. */ R *= (ekf_rate_ / pose_rate_); ekf_.updateWithDelay(y, C, R, delay_step); // debug Eigen::MatrixXd X_result(dim_x_, 1); ekf_.getLatestX(X_result); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); } /* * measurementUpdateTwist */ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::TwistStamped& twist) { if (twist.header.frame_id != output_frame_id_) { ROS_WARN_DELAYED_THROTTLE(2.0, "twist frame_id must be %s", output_frame_id_.c_str()); } Eigen::MatrixXd X_curr(dim_x_, 1); // curent state ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); constexpr int dim_y = 2; // vx, wz const ros::Time t_curr = ros::Time::now(); /* Calculate delay step */ double delay_time = (t_curr - twist.header.stamp).toSec() + twist_additional_delay_; if (delay_time < 0.0) { ROS_WARN_DELAYED_THROTTLE(1.0, "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time); delay_time = 0.0; } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { ROS_WARN_DELAYED_THROTTLE(1.0, "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); return; } DEBUG_INFO("delay_time: %f [s]", delay_time); /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << twist.twist.linear.x, twist.twist.angular.z; if (isnan(y.array()).any() || isinf(y.array()).any()) { ROS_WARN("[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); return; } /* Gate */ Eigen::MatrixXd y_ekf(dim_y, 1); y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ); Eigen::MatrixXd P_curr, P_y; ekf_.getLatestP(P_curr); P_y = P_curr.block(4, 4, dim_y, dim_y); if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) { ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; } DEBUG_PRINT_MAT(y.transpose()); DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); /* Set measurement matrix */ Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); C(0, IDX::VX) = 1.0; // for vx C(1, IDX::WZ) = 1.0; // for wz /* Set measurement noise covariancs */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (use_twist_with_covariance_) { R(0, 0) = current_twist_covariance_.at(0); // vx - vx R(0, 1) = current_twist_covariance_.at(5); // vx - wz R(1, 0) = current_twist_covariance_.at(30); // wz - vx R(1, 1) = current_twist_covariance_.at(35); // wz - wz } else { R(0, 0) = twist_stddev_vx_ * twist_stddev_vx_ * ekf_dt_; // for vx R(1, 1) = twist_stddev_wz_ * twist_stddev_wz_ * ekf_dt_; // for wz } /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */ R *= (ekf_rate_ / twist_rate_); ekf_.updateWithDelay(y, C, R, delay_step); // debug Eigen::MatrixXd X_result(dim_x_, 1); ekf_.getLatestX(X_result); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); }; /* * mahalanobisGate */ bool EKFLocalizer::mahalanobisGate(const double& dist_max, const Eigen::MatrixXd& x, const Eigen::MatrixXd& obj_x, const Eigen::MatrixXd& cov) { Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x); // DEBUG_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max); ROS_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max); if (mahalanobis_squared(0) > dist_max * dist_max) { return false; } return true; } /* * publishEstimateResult */ void EKFLocalizer::publishEstimateResult() { ros::Time current_time = ros::Time::now(); Eigen::MatrixXd X(dim_x_, 1); Eigen::MatrixXd P(dim_x_, dim_x_); ekf_.getLatestX(X); ekf_.getLatestP(P); /* publish latest pose */ pub_pose_.publish(current_ekf_pose_); /* publish latest pose with covariance */ geometry_msgs::PoseWithCovarianceStamped pose_cov; pose_cov.header.stamp = current_time; pose_cov.header.frame_id = current_ekf_pose_.header.frame_id; pose_cov.pose.pose = current_ekf_pose_.pose; pose_cov.pose.covariance[0] = P(IDX::X, IDX::X); pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y); pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW); pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X); pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y); pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW); pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X); pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y); pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW); pub_pose_cov_.publish(pose_cov); /* publish latest twist */ pub_twist_.publish(current_ekf_twist_); /* publish latest twist with covariance */ geometry_msgs::TwistWithCovarianceStamped twist_cov; twist_cov.header.stamp = current_time; twist_cov.header.frame_id = current_ekf_twist_.header.frame_id; twist_cov.twist.twist = current_ekf_twist_.twist; twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX); twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ) + P(IDX::VX, IDX::WZ_IMU); twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX) + P(IDX::WZ_IMU, IDX::VX); twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ) + P(IDX::WZ, IDX::WZ_IMU) + P(IDX::WZ_IMU, IDX::WZ) + P(IDX::WZ_IMU, IDX::WZ_IMU); pub_twist_cov_.publish(twist_cov); /* Send transform of pose */ broadcastTF(current_time); /* publish yaw bias */ std_msgs::Float64 yawb; yawb.data = X(IDX::YAWB); pub_yaw_bias_.publish(yawb); /* debug measured pose */ if (current_pose_ptr_ != nullptr) { geometry_msgs::PoseStamped p; p = *current_pose_ptr_; p.header.stamp = current_time; pub_measured_pose_.publish(p); } /* debug publish */ double RAD2DEG = 180.0 / 3.141592; double pose_yaw = 0.0; if (current_pose_ptr_ != nullptr) pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation) * RAD2DEG; std_msgs::Float64MultiArray msg; msg.data.push_back(X(IDX::YAW) * RAD2DEG); // [0] ekf yaw angle msg.data.push_back(pose_yaw); // [1] measurement yaw angle msg.data.push_back(X(IDX::YAWB) * RAD2DEG); // [2] yaw bias msg.data.push_back(X(IDX::AX)); // [3] estimated x acceleration msg.data.push_back(X(IDX::WZ_IMU)); // [4] estimated wz from IMU pub_debug_.publish(msg); } double EKFLocalizer::normalizeYaw(const double& yaw) { return std::atan2(std::sin(yaw), std::cos(yaw)); } /* * measurementUpdateIMU */ void EKFLocalizer::measurementUpdateIMU(const sensor_msgs::Imu& imu) { Eigen::MatrixXd X_curr(dim_x_, 1); ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); constexpr int dim_y = 2; // ax, wz const ros::Time t_curr = ros::Time::now(); /* Calculate delay step */ double delay_time = (t_curr - imu.header.stamp).toSec(); if (delay_time < 0.0) { ROS_WARN_DELAYED_THROTTLE(1.0, "IMU time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); delay_time = 0.0; } int delay_step = std::roundf(delay_time / ekf_dt_); if (delay_step > extend_state_step_ - 1) { ROS_WARN_DELAYED_THROTTLE(1.0, "IMU delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", delay_time, extend_state_step_ * ekf_dt_); return; } DEBUG_INFO("delay_time: %f [s]", delay_time); /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << imu.linear_acceleration.x, imu.angular_velocity.z; if (isnan(y.array()).any() || isinf(y.array()).any()) { ROS_WARN("[EKF] IMU measurement matrix includes NaN or Inf. ignore update. check IMU message."); return; } /* Gate */ Eigen::MatrixXd y_ekf(dim_y, 1); y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::AX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ_IMU); Eigen::MatrixXd P_curr, P_y; ekf_.getLatestP(P_curr); P_y = P_curr.block(IDX::AX, IDX::AX, dim_y, dim_y); if (!mahalanobisGate(imu_gate_dist_, y_ekf, y, P_y)) { ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] IMU measurement update, mahalanobis distance is over limit. ignore " "measurement data."); return; } DEBUG_PRINT_MAT(y.transpose()); DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); /* Set measurement matrix */ Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); C(0, IDX::AX) = 1.0; // for ax C(1, IDX::WZ_IMU) = 1.0; // for wz_imu /* Set measurement noise covariance */ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); R(0, 0) = imu_stddev_ax_ * imu_stddev_ax_; // for ax R(1, 1) = imu_stddev_wz_ * imu_stddev_wz_; // for wz_imu /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */ R *= (ekf_rate_ / imu_rate_); ekf_.updateWithDelay(y, C, R, delay_step); // debug Eigen::MatrixXd X_result(dim_x_, 1); ekf_.getLatestX(X_result); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); // Publish debug IMU message if (pub_measured_imu_.getNumSubscribers() > 0) { sensor_msgs::Imu debug_imu = imu; debug_imu.header.stamp = t_curr; pub_measured_imu_.publish(debug_imu); } } 小车转向时ekf发布的base_link在rviz中的转向不变但点云在转动,而且小车相对点云位置准确而且稳定
08-10
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值